Tesseract
Motion Planning Environment
iiwa7_ikfast_solver.hpp
Go to the documentation of this file.
1 // clang-format off
2 
23 #include <tesseract_common/macros.h>
24 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
25 #ifndef IKFAST_HAS_LIBRARY
26 #define IKFAST_HAS_LIBRARY
27 #define IKFAST_NO_MAIN
28 #endif
29 #include <tesseract_kinematics/ikfast/external/ikfast.h> // found inside share/openrave-X.Y/python/ikfast.h
30 using namespace ikfast;
31 
32 // check if the included ikfast version matches what this file was compiled with
33 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
34 IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61);
35 
36 #include <cmath>
37 #include <vector>
38 #include <limits>
39 #include <algorithm>
40 #include <complex>
41 
42 #ifndef IKFAST_ASSERT
43 #include <stdexcept>
44 #include <sstream>
45 #include <iostream>
46 
47 #ifdef _MSC_VER
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __FUNCDNAME__
50 #endif
51 #endif
52 
53 #ifndef __PRETTY_FUNCTION__
54 #define __PRETTY_FUNCTION__ __func__
55 #endif
56 
57 #define IKFAST_ASSERT(b) \
58  { \
59  if (!(b)) \
60  { \
61  std::stringstream ss; \
62  ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
63  << #b << "' failed"; \
64  throw std::runtime_error(ss.str()); \
65  } \
66  }
67 
68 #endif
69 
70 #if defined(_MSC_VER)
71 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
72 #else
73 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
74 #endif
75 
76 #define IK2PI ((IkReal)6.28318530717959)
77 #define IKPI ((IkReal)3.14159265358979)
78 #define IKPI_2 ((IkReal)1.57079632679490)
79 
80 #ifdef _MSC_VER
81 #ifndef isnan
82 #define isnan _isnan
83 #endif
84 #ifndef isinf
85 #define isinf _isinf
86 #endif
87 //#ifndef isfinite
88 //#define isfinite _isfinite
89 //#endif
90 #endif // _MSC_VER
91 
92 // lapack routines
93 extern "C" {
94 void dgetrf_(const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
95 void zgetrf_(const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
96 void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
97 void dgesv_(const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
98 void dgetrs_(const char* trans,
99  const int* n,
100  const int* nrhs,
101  double* a,
102  const int* lda,
103  int* ipiv,
104  double* b,
105  const int* ldb,
106  int* info);
107 void dgeev_(const char* jobvl,
108  const char* jobvr,
109  const int* n,
110  double* a,
111  const int* lda,
112  double* wr,
113  double* wi,
114  double* vl,
115  const int* ldvl,
116  double* vr,
117  const int* ldvr,
118  double* work,
119  const int* lwork,
120  int* info);
121 }
122 
123 using namespace std; // necessary to get std math routines
124 
125 #ifdef IKFAST_NAMESPACE
126 namespace IKFAST_NAMESPACE
127 {
128 #endif
129 
130 inline float IKabs(float f) { return fabsf(f); }
131 inline double IKabs(double f) { return fabs(f); }
132 
133 inline float IKsqr(float f) { return f * f; }
134 inline double IKsqr(double f) { return f * f; }
135 
136 inline float IKlog(float f) { return logf(f); }
137 inline double IKlog(double f) { return log(f); }
138 
139 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
140 #ifndef IKFAST_SINCOS_THRESH
141 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
142 #endif
143 
144 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and
145 // evaluation
146 #ifndef IKFAST_ATAN2_MAGTHRESH
147 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
148 #endif
149 
150 // minimum distance of separate solutions
151 #ifndef IKFAST_SOLUTION_THRESH
152 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
153 #endif
154 
155 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they
156 // can deviate
157 #ifndef IKFAST_EVALCOND_THRESH
158 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
159 #endif
160 
161 inline float IKasin(float f)
162 {
163  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
164  // wrong with the solver
165  if (f <= -1)
166  return float(-IKPI_2);
167  else if (f >= 1)
168  return float(IKPI_2);
169  return asinf(f);
170 }
171 inline double IKasin(double f)
172 {
173  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
174  // wrong with the solver
175  if (f <= -1)
176  return -IKPI_2;
177  else if (f >= 1)
178  return IKPI_2;
179  return asin(f);
180 }
181 
182 // return positive value in [0,y)
183 inline float IKfmod(float x, float y)
184 {
185  while (x < 0)
186  {
187  x += y;
188  }
189  return fmodf(x, y);
190 }
191 
192 // return positive value in [0,y)
193 inline double IKfmod(double x, double y)
194 {
195  while (x < 0)
196  {
197  x += y;
198  }
199  return fmod(x, y);
200 }
201 
202 inline float IKacos(float f)
203 {
204  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
205  // wrong with the solver
206  if (f <= -1)
207  return float(IKPI);
208  else if (f >= 1)
209  return float(0);
210  return acosf(f);
211 }
212 inline double IKacos(double f)
213 {
214  IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
215  // wrong with the solver
216  if (f <= -1)
217  return IKPI;
218  else if (f >= 1)
219  return 0;
220  return acos(f);
221 }
222 inline float IKsin(float f) { return sinf(f); }
223 inline double IKsin(double f) { return sin(f); }
224 inline float IKcos(float f) { return cosf(f); }
225 inline double IKcos(double f) { return cos(f); }
226 inline float IKtan(float f) { return tanf(f); }
227 inline double IKtan(double f) { return tan(f); }
228 inline float IKsqrt(float f)
229 {
230  if (f <= 0.0f)
231  return 0.0f;
232  return sqrtf(f);
233 }
234 inline double IKsqrt(double f)
235 {
236  if (f <= 0.0)
237  return 0.0;
238  return sqrt(f);
239 }
240 inline float IKatan2Simple(float fy, float fx) { return atan2f(fy, fx); }
241 inline float IKatan2(float fy, float fx)
242 {
243  if (isnan(fy))
244  {
245  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
246  return float(IKPI_2);
247  }
248  else if (isnan(fx))
249  {
250  return 0;
251  }
252  return atan2f(fy, fx);
253 }
254 inline double IKatan2Simple(double fy, double fx) { return atan2(fy, fx); }
255 inline double IKatan2(double fy, double fx)
256 {
257  if (isnan(fy))
258  {
259  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
260  return IKPI_2;
261  }
262  else if (isnan(fx))
263  {
264  return 0;
265  }
266  return atan2(fy, fx);
267 }
268 
269 template <typename T>
270 struct CheckValue
271 {
272  T value;
273  bool valid;
274 };
275 
276 template <typename T>
277 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
278 {
279  CheckValue<T> ret;
280  ret.valid = false;
281  ret.value = 0;
282  if (!isnan(fy) && !isnan(fx))
283  {
284  if (IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH)
285  {
286  ret.value = IKatan2Simple(fy, fx);
287  ret.valid = true;
288  }
289  }
290  return ret;
291 }
292 
293 inline float IKsign(float f)
294 {
295  if (f > 0)
296  {
297  return float(1);
298  }
299  else if (f < 0)
300  {
301  return float(-1);
302  }
303  return 0;
304 }
305 
306 inline double IKsign(double f)
307 {
308  if (f > 0)
309  {
310  return 1.0;
311  }
312  else if (f < 0)
313  {
314  return -1.0;
315  }
316  return 0;
317 }
318 
319 template <typename T>
320 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
321 {
322  CheckValue<T> ret;
323  ret.valid = true;
324  if (n == 0)
325  {
326  ret.value = 1.0;
327  return ret;
328  }
329  else if (n == 1)
330  {
331  ret.value = f;
332  return ret;
333  }
334  else if (n < 0)
335  {
336  if (f == 0)
337  {
338  ret.valid = false;
339  ret.value = (T)1.0e30;
340  return ret;
341  }
342  if (n == -1)
343  {
344  ret.value = T(1.0) / f;
345  return ret;
346  }
347  }
348 
349  int num = n > 0 ? n : -n;
350  if (num == 2)
351  {
352  ret.value = f * f;
353  }
354  else if (num == 3)
355  {
356  ret.value = f * f * f;
357  }
358  else
359  {
360  ret.value = 1.0;
361  while (num > 0)
362  {
363  if (num & 1)
364  {
365  ret.value *= f;
366  }
367  num >>= 1;
368  f *= f;
369  }
370  }
371 
372  if (n < 0)
373  {
374  ret.value = T(1.0) / ret.value;
375  }
376  return ret;
377 }
378 
381 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
382 {
383  IkReal x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15, x16, x17, x18, x19, x20, x21, x22, x23,
384  x24, x25, x26, x27, x28, x29, x30, x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45, x46,
385  x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, x58, x59, x60, x61, x62, x63, x64, x65, x66, x67, x68;
386  x0 = IKcos(j[0]);
387  x1 = IKcos(j[1]);
388  x2 = IKcos(j[2]);
389  x3 = IKsin(j[0]);
390  x4 = IKsin(j[2]);
391  x5 = IKcos(j[3]);
392  x6 = IKsin(j[1]);
393  x7 = IKsin(j[3]);
394  x8 = IKcos(j[4]);
395  x9 = IKsin(j[4]);
396  x10 = IKcos(j[6]);
397  x11 = IKsin(j[6]);
398  x12 = IKsin(j[5]);
399  x13 = IKcos(j[5]);
400  x14 = ((0.4) * x3);
401  x15 = ((0.15085) * x5);
402  x16 = ((1.0) * x5);
403  x17 = ((1.0) * x8);
404  x18 = ((1.0) * x7);
405  x19 = ((1.0) * x4);
406  x20 = ((1.0) * x13);
407  x21 = ((1.0) * x1);
408  x22 = ((0.15085) * x7);
409  x23 = ((0.15085) * x8);
410  x24 = ((0.4) * x5);
411  x25 = ((0.4) * x2);
412  x26 = ((1.0) * x9);
413  x27 = ((0.15085) * x9);
414  x28 = (x0 * x6);
415  x29 = (x0 * x4);
416  x30 = ((-1.0) * x9);
417  x31 = (x5 * x6);
418  x32 = (x6 * x9);
419  x33 = (x2 * x3);
420  x34 = ((-1.0) * x8);
421  x35 = (x6 * x7);
422  x36 = (x1 * x2);
423  x37 = (x0 * x2);
424  x38 = (x2 * x6);
425  x39 = (x3 * x6);
426  x40 = (x19 * x3);
427  x41 = (x16 * x38);
428  x42 = (x19 * x32);
429  x43 = ((((-1.0) * x40)) + ((x0 * x36)));
430  x44 = (x29 + ((x1 * x33)));
431  x45 = (((x1 * x5)) + ((x2 * x35)));
432  x46 = (x40 + (((-1.0) * x21 * x37)));
433  x47 = ((((-1.0) * x37)) + ((x1 * x40)));
434  x48 = ((((-1.0) * x1 * x18)) + x41);
435  x49 = (((x0 * x1 * x19)) + (((1.0) * x33)));
436  x50 = ((((-1.0) * x0 * x19)) + (((-1.0) * x21 * x33)));
437  x51 = (x12 * x45);
438  x52 = (x43 * x5);
439  x53 = (x48 * x8);
440  x54 = (x47 * x9);
441  x55 = (x49 * x9);
442  x56 = (x50 * x7);
443  x57 = (x52 + ((x28 * x7)));
444  x58 = (((x3 * x35)) + ((x44 * x5)));
445  x59 = (((x46 * x7)) + ((x28 * x5)));
446  x60 = ((((-1.0) * x42)) + x53);
447  x61 = (((x26 * (((((-1.0) * x1 * x7)) + x41)))) + ((x17 * x4 * x6)));
448  x62 = ((((-1.0) * x18 * x28)) + (((-1.0) * x16 * x43)));
449  x63 = (((x3 * x31)) + x56);
450  x64 = ((((-1.0) * x18 * x39)) + (((-1.0) * x16 * x44)));
451  x65 = (x12 * x59);
452  x66 = (x62 * x8);
453  x67 = (x12 * x63);
454  x68 = (x54 + ((x64 * x8)));
455  eerot[0] =
456  ((((-1.0) * x10 *
457  (((((1.0) * x65)) + (((1.0) * x13 * ((((x8 * (((((-1.0) * x28 * x7)) + (((-1.0) * x52)))))) + x55)))))))) +
458  ((x11 * (((((-1.0) * x17 * x49)) + (((-1.0) * x26 * x57)))))));
459  eerot[1] = (((x11 * ((((x13 * ((x55 + x66)))) + x65)))) + ((x10 * ((((x34 * x49)) + ((x30 * x57)))))));
460  eerot[2] = (((x13 * x59)) + ((x12 * (((((-1.0) * x17 * x62)) + (((-1.0) * x26 * x49)))))));
461  eetrans[0] = (((x7 * (((((-1.0) * x0 * x1 * x25)) + ((x14 * x4)))))) + ((x13 * ((((x22 * x46)) + ((x15 * x28)))))) +
462  (((0.4) * x28)) + ((x12 * (((((-1.0) * x27 * x49)) + (((-1.0) * x23 * x62)))))) + ((x24 * x28)));
463  eerot[3] = (((x10 * (((((-1.0) * x20 * x68)) + (((-1.0) * x67)))))) + ((x11 * ((((x34 * x47)) + ((x30 * x58)))))));
464  eerot[4] = (((x10 * (((((-1.0) * x17 * x47)) + (((-1.0) * x26 * x58)))))) + ((x11 * ((((x13 * x68)) + x67)))));
465  eerot[5] = (((x13 * x63)) + ((x12 * (((((-1.0) * x17 * x64)) + (((-1.0) * x26 * x47)))))));
466  eetrans[1] = (((x13 * ((((x15 * x39)) + ((x22 * x50)))))) + ((x7 * (((((-1.0) * x14 * x36)) + (((-0.4) * x29)))))) +
467  ((x14 * x6)) + ((x14 * x31)) + ((x12 * (((((-1.0) * x27 * x47)) + (((-1.0) * x23 * x64)))))));
468  eerot[6] = (((x11 * x61)) + ((x10 * (((((-1.0) * x20 * x60)) + (((-1.0) * x51)))))));
469  eerot[7] = (((x11 * ((((x13 * x60)) + x51)))) + ((x10 * x61)));
470  eerot[8] = (((x12 * (((((-1.0) * x17 * x48)) + x42)))) + ((x13 * x45)));
471  eetrans[2] = ((0.34) + ((x1 * x24)) + (((0.4) * x1)) + ((x12 * ((((x27 * x4 * x6)) + (((-1.0) * x23 * x48)))))) +
472  ((x13 * ((((x1 * x15)) + ((x22 * x38)))))) + ((x25 * x35)));
473 }
474 
475 IKFAST_API int GetNumFreeParameters() { return 1; }
476 IKFAST_API int* GetFreeParameters()
477 {
478  static int freeparams[] = { 4 };
479  return freeparams;
480 }
481 IKFAST_API int GetNumJoints() { return 7; }
482 
483 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
484 
485 IKFAST_API int GetIkType() { return 0x67000001; }
486 
487 class IKSolver
488 {
489 public:
490  IkReal j0, cj0, sj0, htj0, j0mul, j1, cj1, sj1, htj1, j1mul, j2, cj2, sj2, htj2, j2mul, j3, cj3, sj3, htj3, j3mul, j5,
491  cj5, sj5, htj5, j5mul, j6, cj6, sj6, htj6, j6mul, j4, cj4, sj4, htj4, new_r00, r00, rxp0_0, new_r01, r01, rxp0_1,
492  new_r02, r02, rxp0_2, new_r10, r10, rxp1_0, new_r11, r11, rxp1_1, new_r12, r12, rxp1_2, new_r20, r20, rxp2_0,
493  new_r21, r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx, new_py, py, npy, new_pz, pz, npz, pp;
494  unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij5[2], _nj5, _ij6[2], _nj6, _ij4[2], _nj4;
495 
496  IkReal j100, cj100, sj100;
497  unsigned char _ij100[2], _nj100;
498  bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
499  {
500  j0 = numeric_limits<IkReal>::quiet_NaN();
501  _ij0[0] = -1;
502  _ij0[1] = -1;
503  _nj0 = -1;
504  j1 = numeric_limits<IkReal>::quiet_NaN();
505  _ij1[0] = -1;
506  _ij1[1] = -1;
507  _nj1 = -1;
508  j2 = numeric_limits<IkReal>::quiet_NaN();
509  _ij2[0] = -1;
510  _ij2[1] = -1;
511  _nj2 = -1;
512  j3 = numeric_limits<IkReal>::quiet_NaN();
513  _ij3[0] = -1;
514  _ij3[1] = -1;
515  _nj3 = -1;
516  j5 = numeric_limits<IkReal>::quiet_NaN();
517  _ij5[0] = -1;
518  _ij5[1] = -1;
519  _nj5 = -1;
520  j6 = numeric_limits<IkReal>::quiet_NaN();
521  _ij6[0] = -1;
522  _ij6[1] = -1;
523  _nj6 = -1;
524  _ij4[0] = -1;
525  _ij4[1] = -1;
526  _nj4 = 0;
527  for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
528  {
529  solutions.Clear();
530  j4 = pfree[0];
531  cj4 = cos(pfree[0]);
532  sj4 = sin(pfree[0]), htj4 = tan(pfree[0] * 0.5);
533  r00 = eerot[0 * 3 + 0];
534  r01 = eerot[0 * 3 + 1];
535  r02 = eerot[0 * 3 + 2];
536  r10 = eerot[1 * 3 + 0];
537  r11 = eerot[1 * 3 + 1];
538  r12 = eerot[1 * 3 + 2];
539  r20 = eerot[2 * 3 + 0];
540  r21 = eerot[2 * 3 + 1];
541  r22 = eerot[2 * 3 + 2];
542  px = eetrans[0];
543  py = eetrans[1];
544  pz = eetrans[2];
545 
546  new_r00 = ((-1.0) * r00);
547  new_r01 = ((-1.0) * r01);
548  new_r02 = r02;
549  new_px = (px + (((-0.15085) * r02)));
550  new_r10 = ((-1.0) * r10);
551  new_r11 = ((-1.0) * r11);
552  new_r12 = r12;
553  new_py = (py + (((-0.15085) * r12)));
554  new_r20 = ((-1.0) * r20);
555  new_r21 = ((-1.0) * r21);
556  new_r22 = r22;
557  new_pz = ((-0.34) + pz + (((-0.15085) * r22)));
558  r00 = new_r00;
559  r01 = new_r01;
560  r02 = new_r02;
561  r10 = new_r10;
562  r11 = new_r11;
563  r12 = new_r12;
564  r20 = new_r20;
565  r21 = new_r21;
566  r22 = new_r22;
567  px = new_px;
568  py = new_py;
569  pz = new_pz;
570  IkReal x69 = ((1.0) * px);
571  IkReal x70 = ((1.0) * pz);
572  IkReal x71 = ((1.0) * py);
573  pp = ((px * px) + (py * py) + (pz * pz));
574  npx = (((px * r00)) + ((py * r10)) + ((pz * r20)));
575  npy = (((px * r01)) + ((py * r11)) + ((pz * r21)));
576  npz = (((px * r02)) + ((py * r12)) + ((pz * r22)));
577  rxp0_0 = ((((-1.0) * r20 * x71)) + ((pz * r10)));
578  rxp0_1 = (((px * r20)) + (((-1.0) * r00 * x70)));
579  rxp0_2 = ((((-1.0) * r10 * x69)) + ((py * r00)));
580  rxp1_0 = ((((-1.0) * r21 * x71)) + ((pz * r11)));
581  rxp1_1 = (((px * r21)) + (((-1.0) * r01 * x70)));
582  rxp1_2 = ((((-1.0) * r11 * x69)) + ((py * r01)));
583  rxp2_0 = ((((-1.0) * r22 * x71)) + ((pz * r12)));
584  rxp2_1 = (((px * r22)) + (((-1.0) * r02 * x70)));
585  rxp2_2 = ((((-1.0) * r12 * x69)) + ((py * r02)));
586  {
587  IkReal j3array[2], cj3array[2], sj3array[2];
588  bool j3valid[2] = { false };
589  _nj3 = 2;
590  cj3array[0] = ((-1.0) + (((3.125) * pp)));
591  if (cj3array[0] >= -1 - IKFAST_SINCOS_THRESH && cj3array[0] <= 1 + IKFAST_SINCOS_THRESH)
592  {
593  j3valid[0] = j3valid[1] = true;
594  j3array[0] = IKacos(cj3array[0]);
595  sj3array[0] = IKsin(j3array[0]);
596  cj3array[1] = cj3array[0];
597  j3array[1] = -j3array[0];
598  sj3array[1] = -sj3array[0];
599  }
600  else if (isnan(cj3array[0]))
601  {
602  // probably any value will work
603  j3valid[0] = true;
604  cj3array[0] = 1;
605  sj3array[0] = 0;
606  j3array[0] = 0;
607  }
608  for (int ij3 = 0; ij3 < 2; ++ij3)
609  {
610  if (!j3valid[ij3])
611  {
612  continue;
613  }
614  _ij3[0] = ij3;
615  _ij3[1] = -1;
616  for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
617  {
618  if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
619  IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
620  {
621  j3valid[iij3] = false;
622  _ij3[1] = iij3;
623  break;
624  }
625  }
626  j3 = j3array[ij3];
627  cj3 = cj3array[ij3];
628  sj3 = sj3array[ij3];
629 
630  {
631  IkReal j6eval[2];
632  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
633  j6eval[1] = ((npx * npx) + (npy * npy));
634  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000)
635  {
636  {
637  IkReal j5eval[2];
638  j5eval[0] = ((1.0) + (cj3 * cj3) + (((2.0) * cj3)) + (((cj4 * cj4) * (sj3 * sj3))));
639  j5eval[1] = ((((2.5) * (IKabs(((0.4) + (((0.4) * cj3))))))) + (IKabs((cj4 * sj3))));
640  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000)
641  {
642  {
643  IkReal evalcond[2];
644  bool bgotonextstatement = true;
645  do
646  {
647  evalcond[0] =
648  ((-3.14159265358979) +
649  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))), 6.28318530717959)));
650  evalcond[1] = pp;
651  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000)
652  {
653  bgotonextstatement = false;
654  {
655  IkReal j6eval[1];
656  sj3 = 0;
657  cj3 = -1.0;
658  j3 = 3.14159265358979;
659  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
660  if (IKabs(j6eval[0]) < 0.0000010000000000)
661  {
662  continue; // no branches [j5, j6]
663  }
664  else
665  {
666  {
667  IkReal j6array[2], cj6array[2], sj6array[2];
668  bool j6valid[2] = { false };
669  _nj6 = 2;
670  CheckValue<IkReal> x73 =
671  IKatan2WithCheck(IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
672  if (!x73.valid)
673  {
674  continue;
675  }
676  IkReal x72 = x73.value;
677  j6array[0] = ((-1.0) * x72);
678  sj6array[0] = IKsin(j6array[0]);
679  cj6array[0] = IKcos(j6array[0]);
680  j6array[1] = ((3.14159265358979) + (((-1.0) * x72)));
681  sj6array[1] = IKsin(j6array[1]);
682  cj6array[1] = IKcos(j6array[1]);
683  if (j6array[0] > IKPI)
684  {
685  j6array[0] -= IK2PI;
686  }
687  else if (j6array[0] < -IKPI)
688  {
689  j6array[0] += IK2PI;
690  }
691  j6valid[0] = true;
692  if (j6array[1] > IKPI)
693  {
694  j6array[1] -= IK2PI;
695  }
696  else if (j6array[1] < -IKPI)
697  {
698  j6array[1] += IK2PI;
699  }
700  j6valid[1] = true;
701  for (int ij6 = 0; ij6 < 2; ++ij6)
702  {
703  if (!j6valid[ij6])
704  {
705  continue;
706  }
707  _ij6[0] = ij6;
708  _ij6[1] = -1;
709  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
710  {
711  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
712  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
713  {
714  j6valid[iij6] = false;
715  _ij6[1] = iij6;
716  break;
717  }
718  }
719  j6 = j6array[ij6];
720  cj6 = cj6array[ij6];
721  sj6 = sj6array[ij6];
722  {
723  IkReal evalcond[1];
724  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) + (((-1.0) * npx * (IKsin(j6)))));
725  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
726  {
727  continue;
728  }
729  }
730 
731  {
732  IkReal j5eval[1];
733  sj3 = 0;
734  cj3 = -1.0;
735  j3 = 3.14159265358979;
736  j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
737  if (IKabs(j5eval[0]) < 0.0000000100000000)
738  {
739  continue; // no branches [j5]
740  }
741  else
742  {
743  IkReal op[2 + 1], zeror[2];
744  int numroots;
745  IkReal x74 = (npy * sj6);
746  IkReal x75 = (cj6 * npx);
747  op[0] = (x75 + (((-1.0) * x74)));
748  op[1] = 0;
749  op[2] = (x74 + (((-1.0) * x75)));
750  polyroots2(op, zeror, numroots);
751  IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
752  int numsolutions = 0;
753  for (int ij5 = 0; ij5 < numroots; ++ij5)
754  {
755  IkReal htj5 = zeror[ij5];
756  tempj5array[0] = ((2.0) * (atan(htj5)));
757  for (int kj5 = 0; kj5 < 1; ++kj5)
758  {
759  j5array[numsolutions] = tempj5array[kj5];
760  if (j5array[numsolutions] > IKPI)
761  {
762  j5array[numsolutions] -= IK2PI;
763  }
764  else if (j5array[numsolutions] < -IKPI)
765  {
766  j5array[numsolutions] += IK2PI;
767  }
768  sj5array[numsolutions] = IKsin(j5array[numsolutions]);
769  cj5array[numsolutions] = IKcos(j5array[numsolutions]);
770  numsolutions++;
771  }
772  }
773  bool j5valid[2] = { true, true };
774  _nj5 = 2;
775  for (int ij5 = 0; ij5 < numsolutions; ++ij5)
776  {
777  if (!j5valid[ij5])
778  {
779  continue;
780  }
781  j5 = j5array[ij5];
782  cj5 = cj5array[ij5];
783  sj5 = sj5array[ij5];
784  htj5 = IKtan(j5 / 2);
785 
786  _ij5[0] = ij5;
787  _ij5[1] = -1;
788  for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
789  {
790  if (j5valid[iij5] &&
791  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
792  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
793  {
794  j5valid[iij5] = false;
795  _ij5[1] = iij5;
796  break;
797  }
798  }
799  rotationfunction0(solutions);
800  }
801  }
802  }
803  }
804  }
805  }
806  }
807  }
808  } while (0);
809  if (bgotonextstatement)
810  {
811  bool bgotonextstatement = true;
812  do
813  {
814  if (1)
815  {
816  bgotonextstatement = false;
817  continue; // branch miss [j5, j6]
818  }
819  } while (0);
820  if (bgotonextstatement)
821  {
822  }
823  }
824  }
825  }
826  else
827  {
828  {
829  IkReal j5array[2], cj5array[2], sj5array[2];
830  bool j5valid[2] = { false };
831  _nj5 = 2;
832  IkReal x1007 = ((0.4) + (((0.4) * cj3)));
833  CheckValue<IkReal> x1010 =
834  IKatan2WithCheck(IkReal(x1007), IkReal(((0.4) * cj4 * sj3)), IKFAST_ATAN2_MAGTHRESH);
835  if (!x1010.valid)
836  {
837  continue;
838  }
839  IkReal x1008 = ((1.0) * (x1010.value));
840  if ((((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007))) < -0.00001)
841  continue;
842  CheckValue<IkReal> x1011 = IKPowWithIntegerCheck(
843  IKabs(IKsqrt(((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007)))), -1);
844  if (!x1011.valid)
845  {
846  continue;
847  }
848  if (((npz * (x1011.value))) < -1 - IKFAST_SINCOS_THRESH ||
849  ((npz * (x1011.value))) > 1 + IKFAST_SINCOS_THRESH)
850  continue;
851  IkReal x1009 = IKasin((npz * (x1011.value)));
852  j5array[0] = (x1009 + (((-1.0) * x1008)));
853  sj5array[0] = IKsin(j5array[0]);
854  cj5array[0] = IKcos(j5array[0]);
855  j5array[1] = ((3.14159265358979) + (((-1.0) * x1009)) + (((-1.0) * x1008)));
856  sj5array[1] = IKsin(j5array[1]);
857  cj5array[1] = IKcos(j5array[1]);
858  if (j5array[0] > IKPI)
859  {
860  j5array[0] -= IK2PI;
861  }
862  else if (j5array[0] < -IKPI)
863  {
864  j5array[0] += IK2PI;
865  }
866  j5valid[0] = true;
867  if (j5array[1] > IKPI)
868  {
869  j5array[1] -= IK2PI;
870  }
871  else if (j5array[1] < -IKPI)
872  {
873  j5array[1] += IK2PI;
874  }
875  j5valid[1] = true;
876  for (int ij5 = 0; ij5 < 2; ++ij5)
877  {
878  if (!j5valid[ij5])
879  {
880  continue;
881  }
882  _ij5[0] = ij5;
883  _ij5[1] = -1;
884  for (int iij5 = ij5 + 1; iij5 < 2; ++iij5)
885  {
886  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
887  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
888  {
889  j5valid[iij5] = false;
890  _ij5[1] = iij5;
891  break;
892  }
893  }
894  j5 = j5array[ij5];
895  cj5 = cj5array[ij5];
896  sj5 = sj5array[ij5];
897 
898  {
899  IkReal j6eval[3];
900  IkReal x1012 = npy * npy;
901  IkReal x1013 = npx * npx;
902  IkReal x1014 = (cj5 * sj4);
903  IkReal x1015 = (cj5 * x1013);
904  IkReal x1016 = (cj5 * x1012);
905  IkReal x1017 = ((5.0) * npz * sj5);
906  IkReal x1018 = ((2.0) * npy * sj3);
907  IkReal x1019 = ((2.0) * npx * sj3);
908  j6eval[0] = (x1015 + x1016);
909  j6eval[1] = ((IKabs((((x1014 * x1019)) + ((cj4 * x1018)) + (((-1.0) * npy * x1017))))) +
910  (IKabs((((x1014 * x1018)) + (((-1.0) * cj4 * x1019)) + ((npx * x1017))))));
911  j6eval[2] = IKsign(((((5.0) * x1016)) + (((5.0) * x1015))));
912  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
913  IKabs(j6eval[2]) < 0.0000010000000000)
914  {
915  {
916  IkReal j6eval[3];
917  IkReal x1020 = npy * npy;
918  IkReal x1021 = npx * npx;
919  IkReal x1022 = ((2.0) * cj3);
920  IkReal x1023 = ((5.0) * sj5);
921  IkReal x1024 = ((2.0) * npx);
922  IkReal x1025 = ((2.0) * npy);
923  IkReal x1026 = ((5.0) * cj5 * npz);
924  IkReal x1027 = (sj3 * sj4 * sj5);
925  j6eval[0] = (((sj5 * x1021)) + ((sj5 * x1020)));
926  j6eval[1] = IKsign((((x1021 * x1023)) + ((x1020 * x1023))));
927  j6eval[2] =
928  ((IKabs((x1024 + ((npx * x1022)) + (((-1.0) * npx * x1026)) + ((x1025 * x1027))))) +
929  (IKabs((((npy * x1026)) + (((-1.0) * x1025)) + (((-1.0) * npy * x1022)) +
930  ((x1024 * x1027))))));
931  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
932  IKabs(j6eval[2]) < 0.0000010000000000)
933  {
934  {
935  IkReal j6eval[3];
936  IkReal x1028 = npx * npx;
937  IkReal x1029 = npy * npy;
938  IkReal x1030 = ((2.0) * sj5);
939  IkReal x1031 = ((2.0) * sj3);
940  IkReal x1032 = (cj4 * cj5);
941  j6eval[0] = (x1029 + x1028);
942  j6eval[1] = ((IKabs(((((-1.0) * npx * x1031 * x1032)) + ((npy * sj4 * x1031)) +
943  ((npx * x1030)) + ((cj3 * npx * x1030))))) +
944  (IKabs((((npx * sj4 * x1031)) + ((npy * x1031 * x1032)) +
945  (((-1.0) * cj3 * npy * x1030)) + (((-1.0) * npy * x1030))))));
946  j6eval[2] = IKsign(((((5.0) * x1028)) + (((5.0) * x1029))));
947  if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
948  IKabs(j6eval[2]) < 0.0000010000000000)
949  {
950  {
951  IkReal evalcond[2];
952  bool bgotonextstatement = true;
953  do
954  {
955  evalcond[0] = ((-3.14159265358979) +
956  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
957  6.28318530717959)));
958  evalcond[1] = pp;
959  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
960  IKabs(evalcond[1]) < 0.0000050000000000)
961  {
962  bgotonextstatement = false;
963  {
964  IkReal j6eval[1];
965  sj3 = 0;
966  cj3 = -1.0;
967  j3 = 3.14159265358979;
968  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
969  if (IKabs(j6eval[0]) < 0.0000010000000000)
970  {
971  {
972  IkReal j6eval[1];
973  sj3 = 0;
974  cj3 = -1.0;
975  j3 = 3.14159265358979;
976  j6eval[0] = ((IKabs((npx * sj5))) + (IKabs((npy * sj5))));
977  if (IKabs(j6eval[0]) < 0.0000010000000000)
978  {
979  {
980  IkReal j6eval[1];
981  sj3 = 0;
982  cj3 = -1.0;
983  j3 = 3.14159265358979;
984  j6eval[0] = ((IKabs((cj5 * npx))) + (IKabs((cj5 * npy))));
985  if (IKabs(j6eval[0]) < 0.0000010000000000)
986  {
987  {
988  IkReal evalcond[1];
989  bool bgotonextstatement = true;
990  do
991  {
992  evalcond[0] = ((-3.14159265358979) +
993  (IKfmod(((3.14159265358979) +
994  (IKabs(((-1.5707963267949) + j5)))),
995  6.28318530717959)));
996  if (IKabs(evalcond[0]) < 0.0000050000000000)
997  {
998  bgotonextstatement = false;
999  {
1000  IkReal j6eval[1];
1001  sj3 = 0;
1002  cj3 = -1.0;
1003  j3 = 3.14159265358979;
1004  sj5 = 1.0;
1005  cj5 = 0;
1006  j5 = 1.5707963267949;
1007  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1008  if (IKabs(j6eval[0]) < 0.0000010000000000)
1009  {
1010  continue; // no branches [j6]
1011  }
1012  else
1013  {
1014  {
1015  IkReal j6array[2], cj6array[2], sj6array[2];
1016  bool j6valid[2] = { false };
1017  _nj6 = 2;
1018  CheckValue<IkReal> x1034 =
1019  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1020  IkReal(npy),
1021  IKFAST_ATAN2_MAGTHRESH);
1022  if (!x1034.valid)
1023  {
1024  continue;
1025  }
1026  IkReal x1033 = x1034.value;
1027  j6array[0] = ((-1.0) * x1033);
1028  sj6array[0] = IKsin(j6array[0]);
1029  cj6array[0] = IKcos(j6array[0]);
1030  j6array[1] = ((3.14159265358979) + (((-1.0) * x1033)));
1031  sj6array[1] = IKsin(j6array[1]);
1032  cj6array[1] = IKcos(j6array[1]);
1033  if (j6array[0] > IKPI)
1034  {
1035  j6array[0] -= IK2PI;
1036  }
1037  else if (j6array[0] < -IKPI)
1038  {
1039  j6array[0] += IK2PI;
1040  }
1041  j6valid[0] = true;
1042  if (j6array[1] > IKPI)
1043  {
1044  j6array[1] -= IK2PI;
1045  }
1046  else if (j6array[1] < -IKPI)
1047  {
1048  j6array[1] += IK2PI;
1049  }
1050  j6valid[1] = true;
1051  for (int ij6 = 0; ij6 < 2; ++ij6)
1052  {
1053  if (!j6valid[ij6])
1054  {
1055  continue;
1056  }
1057  _ij6[0] = ij6;
1058  _ij6[1] = -1;
1059  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1060  {
1061  if (j6valid[iij6] &&
1062  IKabs(cj6array[ij6] - cj6array[iij6]) <
1063  IKFAST_SOLUTION_THRESH &&
1064  IKabs(sj6array[ij6] - sj6array[iij6]) <
1065  IKFAST_SOLUTION_THRESH)
1066  {
1067  j6valid[iij6] = false;
1068  _ij6[1] = iij6;
1069  break;
1070  }
1071  }
1072  j6 = j6array[ij6];
1073  cj6 = cj6array[ij6];
1074  sj6 = sj6array[ij6];
1075  {
1076  IkReal evalcond[1];
1077  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1078  (((-1.0) * npx * (IKsin(j6)))));
1079  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1080  {
1081  continue;
1082  }
1083  }
1084 
1085  rotationfunction0(solutions);
1086  }
1087  }
1088  }
1089  }
1090  }
1091  } while (0);
1092  if (bgotonextstatement)
1093  {
1094  bool bgotonextstatement = true;
1095  do
1096  {
1097  evalcond[0] = ((-3.14159265358979) +
1098  (IKfmod(((3.14159265358979) +
1099  (IKabs(((1.5707963267949) + j5)))),
1100  6.28318530717959)));
1101  if (IKabs(evalcond[0]) < 0.0000050000000000)
1102  {
1103  bgotonextstatement = false;
1104  {
1105  IkReal j6eval[1];
1106  sj3 = 0;
1107  cj3 = -1.0;
1108  j3 = 3.14159265358979;
1109  sj5 = -1.0;
1110  cj5 = 0;
1111  j5 = -1.5707963267949;
1112  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1113  if (IKabs(j6eval[0]) < 0.0000010000000000)
1114  {
1115  continue; // no branches [j6]
1116  }
1117  else
1118  {
1119  {
1120  IkReal j6array[2], cj6array[2], sj6array[2];
1121  bool j6valid[2] = { false };
1122  _nj6 = 2;
1123  CheckValue<IkReal> x1036 =
1124  IKatan2WithCheck(IkReal(npx),
1125  IkReal(((-1.0) * npy)),
1126  IKFAST_ATAN2_MAGTHRESH);
1127  if (!x1036.valid)
1128  {
1129  continue;
1130  }
1131  IkReal x1035 = x1036.value;
1132  j6array[0] = ((-1.0) * x1035);
1133  sj6array[0] = IKsin(j6array[0]);
1134  cj6array[0] = IKcos(j6array[0]);
1135  j6array[1] =
1136  ((3.14159265358979) + (((-1.0) * x1035)));
1137  sj6array[1] = IKsin(j6array[1]);
1138  cj6array[1] = IKcos(j6array[1]);
1139  if (j6array[0] > IKPI)
1140  {
1141  j6array[0] -= IK2PI;
1142  }
1143  else if (j6array[0] < -IKPI)
1144  {
1145  j6array[0] += IK2PI;
1146  }
1147  j6valid[0] = true;
1148  if (j6array[1] > IKPI)
1149  {
1150  j6array[1] -= IK2PI;
1151  }
1152  else if (j6array[1] < -IKPI)
1153  {
1154  j6array[1] += IK2PI;
1155  }
1156  j6valid[1] = true;
1157  for (int ij6 = 0; ij6 < 2; ++ij6)
1158  {
1159  if (!j6valid[ij6])
1160  {
1161  continue;
1162  }
1163  _ij6[0] = ij6;
1164  _ij6[1] = -1;
1165  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1166  {
1167  if (j6valid[iij6] &&
1168  IKabs(cj6array[ij6] - cj6array[iij6]) <
1169  IKFAST_SOLUTION_THRESH &&
1170  IKabs(sj6array[ij6] - sj6array[iij6]) <
1171  IKFAST_SOLUTION_THRESH)
1172  {
1173  j6valid[iij6] = false;
1174  _ij6[1] = iij6;
1175  break;
1176  }
1177  }
1178  j6 = j6array[ij6];
1179  cj6 = cj6array[ij6];
1180  sj6 = sj6array[ij6];
1181  {
1182  IkReal evalcond[1];
1183  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1184  (((-1.0) * npx * (IKsin(j6)))));
1185  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1186  {
1187  continue;
1188  }
1189  }
1190 
1191  rotationfunction0(solutions);
1192  }
1193  }
1194  }
1195  }
1196  }
1197  } while (0);
1198  if (bgotonextstatement)
1199  {
1200  bool bgotonextstatement = true;
1201  do
1202  {
1203  evalcond[0] = ((-3.14159265358979) +
1204  (IKfmod(((3.14159265358979) + (IKabs(j5))),
1205  6.28318530717959)));
1206  if (IKabs(evalcond[0]) < 0.0000050000000000)
1207  {
1208  bgotonextstatement = false;
1209  {
1210  IkReal j6eval[1];
1211  sj3 = 0;
1212  cj3 = -1.0;
1213  j3 = 3.14159265358979;
1214  sj5 = 0;
1215  cj5 = 1.0;
1216  j5 = 0;
1217  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1218  if (IKabs(j6eval[0]) < 0.0000010000000000)
1219  {
1220  continue; // no branches [j6]
1221  }
1222  else
1223  {
1224  {
1225  IkReal j6array[2], cj6array[2], sj6array[2];
1226  bool j6valid[2] = { false };
1227  _nj6 = 2;
1228  CheckValue<IkReal> x1038 =
1229  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1230  IkReal(npy),
1231  IKFAST_ATAN2_MAGTHRESH);
1232  if (!x1038.valid)
1233  {
1234  continue;
1235  }
1236  IkReal x1037 = x1038.value;
1237  j6array[0] = ((-1.0) * x1037);
1238  sj6array[0] = IKsin(j6array[0]);
1239  cj6array[0] = IKcos(j6array[0]);
1240  j6array[1] =
1241  ((3.14159265358979) + (((-1.0) * x1037)));
1242  sj6array[1] = IKsin(j6array[1]);
1243  cj6array[1] = IKcos(j6array[1]);
1244  if (j6array[0] > IKPI)
1245  {
1246  j6array[0] -= IK2PI;
1247  }
1248  else if (j6array[0] < -IKPI)
1249  {
1250  j6array[0] += IK2PI;
1251  }
1252  j6valid[0] = true;
1253  if (j6array[1] > IKPI)
1254  {
1255  j6array[1] -= IK2PI;
1256  }
1257  else if (j6array[1] < -IKPI)
1258  {
1259  j6array[1] += IK2PI;
1260  }
1261  j6valid[1] = true;
1262  for (int ij6 = 0; ij6 < 2; ++ij6)
1263  {
1264  if (!j6valid[ij6])
1265  {
1266  continue;
1267  }
1268  _ij6[0] = ij6;
1269  _ij6[1] = -1;
1270  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1271  {
1272  if (j6valid[iij6] &&
1273  IKabs(cj6array[ij6] - cj6array[iij6]) <
1274  IKFAST_SOLUTION_THRESH &&
1275  IKabs(sj6array[ij6] - sj6array[iij6]) <
1276  IKFAST_SOLUTION_THRESH)
1277  {
1278  j6valid[iij6] = false;
1279  _ij6[1] = iij6;
1280  break;
1281  }
1282  }
1283  j6 = j6array[ij6];
1284  cj6 = cj6array[ij6];
1285  sj6 = sj6array[ij6];
1286  {
1287  IkReal evalcond[1];
1288  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1289  (((-1.0) * npx * (IKsin(j6)))));
1290  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1291  {
1292  continue;
1293  }
1294  }
1295 
1296  rotationfunction0(solutions);
1297  }
1298  }
1299  }
1300  }
1301  }
1302  } while (0);
1303  if (bgotonextstatement)
1304  {
1305  bool bgotonextstatement = true;
1306  do
1307  {
1308  evalcond[0] =
1309  ((-3.14159265358979) +
1310  (IKfmod(((3.14159265358979) +
1311  (IKabs(((-3.14159265358979) + j5)))),
1312  6.28318530717959)));
1313  if (IKabs(evalcond[0]) < 0.0000050000000000)
1314  {
1315  bgotonextstatement = false;
1316  {
1317  IkReal j6eval[1];
1318  sj3 = 0;
1319  cj3 = -1.0;
1320  j3 = 3.14159265358979;
1321  sj5 = 0;
1322  cj5 = -1.0;
1323  j5 = 3.14159265358979;
1324  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1325  if (IKabs(j6eval[0]) < 0.0000010000000000)
1326  {
1327  continue; // no branches [j6]
1328  }
1329  else
1330  {
1331  {
1332  IkReal j6array[2], cj6array[2], sj6array[2];
1333  bool j6valid[2] = { false };
1334  _nj6 = 2;
1335  CheckValue<IkReal> x1040 =
1336  IKatan2WithCheck(IkReal(npx),
1337  IkReal(((-1.0) * npy)),
1338  IKFAST_ATAN2_MAGTHRESH);
1339  if (!x1040.valid)
1340  {
1341  continue;
1342  }
1343  IkReal x1039 = x1040.value;
1344  j6array[0] = ((-1.0) * x1039);
1345  sj6array[0] = IKsin(j6array[0]);
1346  cj6array[0] = IKcos(j6array[0]);
1347  j6array[1] =
1348  ((3.14159265358979) + (((-1.0) * x1039)));
1349  sj6array[1] = IKsin(j6array[1]);
1350  cj6array[1] = IKcos(j6array[1]);
1351  if (j6array[0] > IKPI)
1352  {
1353  j6array[0] -= IK2PI;
1354  }
1355  else if (j6array[0] < -IKPI)
1356  {
1357  j6array[0] += IK2PI;
1358  }
1359  j6valid[0] = true;
1360  if (j6array[1] > IKPI)
1361  {
1362  j6array[1] -= IK2PI;
1363  }
1364  else if (j6array[1] < -IKPI)
1365  {
1366  j6array[1] += IK2PI;
1367  }
1368  j6valid[1] = true;
1369  for (int ij6 = 0; ij6 < 2; ++ij6)
1370  {
1371  if (!j6valid[ij6])
1372  {
1373  continue;
1374  }
1375  _ij6[0] = ij6;
1376  _ij6[1] = -1;
1377  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1378  {
1379  if (j6valid[iij6] &&
1380  IKabs(cj6array[ij6] - cj6array[iij6]) <
1381  IKFAST_SOLUTION_THRESH &&
1382  IKabs(sj6array[ij6] - sj6array[iij6]) <
1383  IKFAST_SOLUTION_THRESH)
1384  {
1385  j6valid[iij6] = false;
1386  _ij6[1] = iij6;
1387  break;
1388  }
1389  }
1390  j6 = j6array[ij6];
1391  cj6 = cj6array[ij6];
1392  sj6 = sj6array[ij6];
1393  {
1394  IkReal evalcond[1];
1395  evalcond[0] =
1396  ((((-1.0) * npy * (IKcos(j6)))) +
1397  (((-1.0) * npx * (IKsin(j6)))));
1398  if (IKabs(evalcond[0]) >
1399  IKFAST_EVALCOND_THRESH)
1400  {
1401  continue;
1402  }
1403  }
1404 
1405  rotationfunction0(solutions);
1406  }
1407  }
1408  }
1409  }
1410  }
1411  } while (0);
1412  if (bgotonextstatement)
1413  {
1414  bool bgotonextstatement = true;
1415  do
1416  {
1417  if (1)
1418  {
1419  bgotonextstatement = false;
1420  continue; // branch miss [j6]
1421  }
1422  } while (0);
1423  if (bgotonextstatement)
1424  {
1425  }
1426  }
1427  }
1428  }
1429  }
1430  }
1431  }
1432  else
1433  {
1434  {
1435  IkReal j6array[2], cj6array[2], sj6array[2];
1436  bool j6valid[2] = { false };
1437  _nj6 = 2;
1438  CheckValue<IkReal> x1042 =
1439  IKatan2WithCheck(IkReal(((-1.0) * cj5 * npx)),
1440  IkReal((cj5 * npy)),
1441  IKFAST_ATAN2_MAGTHRESH);
1442  if (!x1042.valid)
1443  {
1444  continue;
1445  }
1446  IkReal x1041 = x1042.value;
1447  j6array[0] = ((-1.0) * x1041);
1448  sj6array[0] = IKsin(j6array[0]);
1449  cj6array[0] = IKcos(j6array[0]);
1450  j6array[1] = ((3.14159265358979) + (((-1.0) * x1041)));
1451  sj6array[1] = IKsin(j6array[1]);
1452  cj6array[1] = IKcos(j6array[1]);
1453  if (j6array[0] > IKPI)
1454  {
1455  j6array[0] -= IK2PI;
1456  }
1457  else if (j6array[0] < -IKPI)
1458  {
1459  j6array[0] += IK2PI;
1460  }
1461  j6valid[0] = true;
1462  if (j6array[1] > IKPI)
1463  {
1464  j6array[1] -= IK2PI;
1465  }
1466  else if (j6array[1] < -IKPI)
1467  {
1468  j6array[1] += IK2PI;
1469  }
1470  j6valid[1] = true;
1471  for (int ij6 = 0; ij6 < 2; ++ij6)
1472  {
1473  if (!j6valid[ij6])
1474  {
1475  continue;
1476  }
1477  _ij6[0] = ij6;
1478  _ij6[1] = -1;
1479  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1480  {
1481  if (j6valid[iij6] &&
1482  IKabs(cj6array[ij6] - cj6array[iij6]) <
1483  IKFAST_SOLUTION_THRESH &&
1484  IKabs(sj6array[ij6] - sj6array[iij6]) <
1485  IKFAST_SOLUTION_THRESH)
1486  {
1487  j6valid[iij6] = false;
1488  _ij6[1] = iij6;
1489  break;
1490  }
1491  }
1492  j6 = j6array[ij6];
1493  cj6 = cj6array[ij6];
1494  sj6 = sj6array[ij6];
1495  {
1496  IkReal evalcond[3];
1497  IkReal x1043 = IKsin(j6);
1498  IkReal x1044 = IKcos(j6);
1499  IkReal x1045 = ((1.0) * npx);
1500  IkReal x1046 = (npy * x1043);
1501  evalcond[0] = (x1046 + (((-1.0) * x1044 * x1045)));
1502  evalcond[1] =
1503  ((((-1.0) * x1043 * x1045)) + (((-1.0) * npy * x1044)));
1504  evalcond[2] =
1505  (((sj5 * x1046)) + (((-1.0) * sj5 * x1044 * x1045)));
1506  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1507  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1508  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1509  {
1510  continue;
1511  }
1512  }
1513 
1514  rotationfunction0(solutions);
1515  }
1516  }
1517  }
1518  }
1519  }
1520  else
1521  {
1522  {
1523  IkReal j6array[2], cj6array[2], sj6array[2];
1524  bool j6valid[2] = { false };
1525  _nj6 = 2;
1526  CheckValue<IkReal> x1048 =
1527  IKatan2WithCheck(IkReal(((-1.0) * npx * sj5)),
1528  IkReal((npy * sj5)),
1529  IKFAST_ATAN2_MAGTHRESH);
1530  if (!x1048.valid)
1531  {
1532  continue;
1533  }
1534  IkReal x1047 = x1048.value;
1535  j6array[0] = ((-1.0) * x1047);
1536  sj6array[0] = IKsin(j6array[0]);
1537  cj6array[0] = IKcos(j6array[0]);
1538  j6array[1] = ((3.14159265358979) + (((-1.0) * x1047)));
1539  sj6array[1] = IKsin(j6array[1]);
1540  cj6array[1] = IKcos(j6array[1]);
1541  if (j6array[0] > IKPI)
1542  {
1543  j6array[0] -= IK2PI;
1544  }
1545  else if (j6array[0] < -IKPI)
1546  {
1547  j6array[0] += IK2PI;
1548  }
1549  j6valid[0] = true;
1550  if (j6array[1] > IKPI)
1551  {
1552  j6array[1] -= IK2PI;
1553  }
1554  else if (j6array[1] < -IKPI)
1555  {
1556  j6array[1] += IK2PI;
1557  }
1558  j6valid[1] = true;
1559  for (int ij6 = 0; ij6 < 2; ++ij6)
1560  {
1561  if (!j6valid[ij6])
1562  {
1563  continue;
1564  }
1565  _ij6[0] = ij6;
1566  _ij6[1] = -1;
1567  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1568  {
1569  if (j6valid[iij6] &&
1570  IKabs(cj6array[ij6] - cj6array[iij6]) <
1571  IKFAST_SOLUTION_THRESH &&
1572  IKabs(sj6array[ij6] - sj6array[iij6]) <
1573  IKFAST_SOLUTION_THRESH)
1574  {
1575  j6valid[iij6] = false;
1576  _ij6[1] = iij6;
1577  break;
1578  }
1579  }
1580  j6 = j6array[ij6];
1581  cj6 = cj6array[ij6];
1582  sj6 = sj6array[ij6];
1583  {
1584  IkReal evalcond[3];
1585  IkReal x1049 = IKsin(j6);
1586  IkReal x1050 = IKcos(j6);
1587  IkReal x1051 = ((1.0) * npx);
1588  IkReal x1052 = (npy * x1049);
1589  evalcond[0] = (x1052 + (((-1.0) * x1050 * x1051)));
1590  evalcond[1] =
1591  ((((-1.0) * npy * x1050)) + (((-1.0) * x1049 * x1051)));
1592  evalcond[2] =
1593  ((((-1.0) * cj5 * x1050 * x1051)) + ((cj5 * x1052)));
1594  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1595  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1596  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1597  {
1598  continue;
1599  }
1600  }
1601 
1602  rotationfunction0(solutions);
1603  }
1604  }
1605  }
1606  }
1607  }
1608  else
1609  {
1610  {
1611  IkReal j6array[2], cj6array[2], sj6array[2];
1612  bool j6valid[2] = { false };
1613  _nj6 = 2;
1614  CheckValue<IkReal> x1054 = IKatan2WithCheck(
1615  IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
1616  if (!x1054.valid)
1617  {
1618  continue;
1619  }
1620  IkReal x1053 = x1054.value;
1621  j6array[0] = ((-1.0) * x1053);
1622  sj6array[0] = IKsin(j6array[0]);
1623  cj6array[0] = IKcos(j6array[0]);
1624  j6array[1] = ((3.14159265358979) + (((-1.0) * x1053)));
1625  sj6array[1] = IKsin(j6array[1]);
1626  cj6array[1] = IKcos(j6array[1]);
1627  if (j6array[0] > IKPI)
1628  {
1629  j6array[0] -= IK2PI;
1630  }
1631  else if (j6array[0] < -IKPI)
1632  {
1633  j6array[0] += IK2PI;
1634  }
1635  j6valid[0] = true;
1636  if (j6array[1] > IKPI)
1637  {
1638  j6array[1] -= IK2PI;
1639  }
1640  else if (j6array[1] < -IKPI)
1641  {
1642  j6array[1] += IK2PI;
1643  }
1644  j6valid[1] = true;
1645  for (int ij6 = 0; ij6 < 2; ++ij6)
1646  {
1647  if (!j6valid[ij6])
1648  {
1649  continue;
1650  }
1651  _ij6[0] = ij6;
1652  _ij6[1] = -1;
1653  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1654  {
1655  if (j6valid[iij6] &&
1656  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
1657  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1658  {
1659  j6valid[iij6] = false;
1660  _ij6[1] = iij6;
1661  break;
1662  }
1663  }
1664  j6 = j6array[ij6];
1665  cj6 = cj6array[ij6];
1666  sj6 = sj6array[ij6];
1667  {
1668  IkReal evalcond[3];
1669  IkReal x1055 = IKsin(j6);
1670  IkReal x1056 = IKcos(j6);
1671  IkReal x1057 = ((1.0) * npx);
1672  IkReal x1058 = (npy * x1055);
1673  evalcond[0] = ((((-1.0) * npy * x1056)) + (((-1.0) * x1055 * x1057)));
1674  evalcond[1] = (((sj5 * x1058)) + (((-1.0) * sj5 * x1056 * x1057)));
1675  evalcond[2] = ((((-1.0) * cj5 * x1056 * x1057)) + ((cj5 * x1058)));
1676  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1677  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1678  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1679  {
1680  continue;
1681  }
1682  }
1683 
1684  rotationfunction0(solutions);
1685  }
1686  }
1687  }
1688  }
1689  }
1690  } while (0);
1691  if (bgotonextstatement)
1692  {
1693  bool bgotonextstatement = true;
1694  do
1695  {
1696  evalcond[0] = ((-3.14159265358979) +
1697  (IKfmod(((3.14159265358979) + (IKabs(j5))), 6.28318530717959)));
1698  if (IKabs(evalcond[0]) < 0.0000050000000000)
1699  {
1700  bgotonextstatement = false;
1701  {
1702  IkReal j6eval[3];
1703  sj5 = 0;
1704  cj5 = 1.0;
1705  j5 = 0;
1706  IkReal x1059 = npx * npx;
1707  IkReal x1060 = npy * npy;
1708  IkReal x1061 = (npy * sj3);
1709  IkReal x1062 = (npx * sj3);
1710  j6eval[0] = (x1059 + x1060);
1711  j6eval[1] = ((IKabs(((((-1.0) * cj4 * x1062)) + ((sj4 * x1061))))) +
1712  (IKabs((((cj4 * x1061)) + ((sj4 * x1062))))));
1713  j6eval[2] = IKsign(((((5.0) * x1060)) + (((5.0) * x1059))));
1714  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1715  IKabs(j6eval[1]) < 0.0000010000000000 ||
1716  IKabs(j6eval[2]) < 0.0000010000000000)
1717  {
1718  {
1719  IkReal evalcond[1];
1720  bool bgotonextstatement = true;
1721  do
1722  {
1723  evalcond[0] =
1724  ((-3.14159265358979) +
1725  (IKfmod(((3.14159265358979) + (IKabs(j3))), 6.28318530717959)));
1726  if (IKabs(evalcond[0]) < 0.0000050000000000)
1727  {
1728  bgotonextstatement = false;
1729  {
1730  IkReal j6eval[1];
1731  sj5 = 0;
1732  cj5 = 1.0;
1733  j5 = 0;
1734  sj3 = 0;
1735  cj3 = 1.0;
1736  j3 = 0;
1737  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1738  if (IKabs(j6eval[0]) < 0.0000010000000000)
1739  {
1740  continue; // no branches [j6]
1741  }
1742  else
1743  {
1744  {
1745  IkReal j6array[2], cj6array[2], sj6array[2];
1746  bool j6valid[2] = { false };
1747  _nj6 = 2;
1748  CheckValue<IkReal> x1064 =
1749  IKatan2WithCheck(IkReal(((-1.0) * npx)),
1750  IkReal(npy),
1751  IKFAST_ATAN2_MAGTHRESH);
1752  if (!x1064.valid)
1753  {
1754  continue;
1755  }
1756  IkReal x1063 = x1064.value;
1757  j6array[0] = ((-1.0) * x1063);
1758  sj6array[0] = IKsin(j6array[0]);
1759  cj6array[0] = IKcos(j6array[0]);
1760  j6array[1] = ((3.14159265358979) + (((-1.0) * x1063)));
1761  sj6array[1] = IKsin(j6array[1]);
1762  cj6array[1] = IKcos(j6array[1]);
1763  if (j6array[0] > IKPI)
1764  {
1765  j6array[0] -= IK2PI;
1766  }
1767  else if (j6array[0] < -IKPI)
1768  {
1769  j6array[0] += IK2PI;
1770  }
1771  j6valid[0] = true;
1772  if (j6array[1] > IKPI)
1773  {
1774  j6array[1] -= IK2PI;
1775  }
1776  else if (j6array[1] < -IKPI)
1777  {
1778  j6array[1] += IK2PI;
1779  }
1780  j6valid[1] = true;
1781  for (int ij6 = 0; ij6 < 2; ++ij6)
1782  {
1783  if (!j6valid[ij6])
1784  {
1785  continue;
1786  }
1787  _ij6[0] = ij6;
1788  _ij6[1] = -1;
1789  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1790  {
1791  if (j6valid[iij6] &&
1792  IKabs(cj6array[ij6] - cj6array[iij6]) <
1793  IKFAST_SOLUTION_THRESH &&
1794  IKabs(sj6array[ij6] - sj6array[iij6]) <
1795  IKFAST_SOLUTION_THRESH)
1796  {
1797  j6valid[iij6] = false;
1798  _ij6[1] = iij6;
1799  break;
1800  }
1801  }
1802  j6 = j6array[ij6];
1803  cj6 = cj6array[ij6];
1804  sj6 = sj6array[ij6];
1805  {
1806  IkReal evalcond[1];
1807  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1808  (((-1.0) * npx * (IKsin(j6)))));
1809  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1810  {
1811  continue;
1812  }
1813  }
1814 
1815  rotationfunction0(solutions);
1816  }
1817  }
1818  }
1819  }
1820  }
1821  } while (0);
1822  if (bgotonextstatement)
1823  {
1824  bool bgotonextstatement = true;
1825  do
1826  {
1827  if (1)
1828  {
1829  bgotonextstatement = false;
1830  continue; // branch miss [j6]
1831  }
1832  } while (0);
1833  if (bgotonextstatement)
1834  {
1835  }
1836  }
1837  }
1838  }
1839  else
1840  {
1841  {
1842  IkReal j6array[1], cj6array[1], sj6array[1];
1843  bool j6valid[1] = { false };
1844  _nj6 = 1;
1845  IkReal x1065 = ((2.0) * sj3);
1846  CheckValue<IkReal> x1066 = IKatan2WithCheck(
1847  IkReal((((cj4 * npy * x1065)) + ((npx * sj4 * x1065)))),
1848  IkReal(((((-1.0) * cj4 * npx * x1065)) + ((npy * sj4 * x1065)))),
1849  IKFAST_ATAN2_MAGTHRESH);
1850  if (!x1066.valid)
1851  {
1852  continue;
1853  }
1854  CheckValue<IkReal> x1067 = IKPowWithIntegerCheck(
1855  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
1856  if (!x1067.valid)
1857  {
1858  continue;
1859  }
1860  j6array[0] = ((-1.5707963267949) + (x1066.value) +
1861  (((1.5707963267949) * (x1067.value))));
1862  sj6array[0] = IKsin(j6array[0]);
1863  cj6array[0] = IKcos(j6array[0]);
1864  if (j6array[0] > IKPI)
1865  {
1866  j6array[0] -= IK2PI;
1867  }
1868  else if (j6array[0] < -IKPI)
1869  {
1870  j6array[0] += IK2PI;
1871  }
1872  j6valid[0] = true;
1873  for (int ij6 = 0; ij6 < 1; ++ij6)
1874  {
1875  if (!j6valid[ij6])
1876  {
1877  continue;
1878  }
1879  _ij6[0] = ij6;
1880  _ij6[1] = -1;
1881  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
1882  {
1883  if (j6valid[iij6] &&
1884  IKabs(cj6array[ij6] - cj6array[iij6]) <
1885  IKFAST_SOLUTION_THRESH &&
1886  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1887  {
1888  j6valid[iij6] = false;
1889  _ij6[1] = iij6;
1890  break;
1891  }
1892  }
1893  j6 = j6array[ij6];
1894  cj6 = cj6array[ij6];
1895  sj6 = sj6array[ij6];
1896  {
1897  IkReal evalcond[2];
1898  IkReal x1068 = IKsin(j6);
1899  IkReal x1069 = IKcos(j6);
1900  IkReal x1070 = ((0.4) * sj3);
1901  IkReal x1071 = ((1.0) * x1069);
1902  evalcond[0] = ((((-1.0) * npx * x1071)) + (((-1.0) * cj4 * x1070)) +
1903  ((npy * x1068)));
1904  evalcond[1] = ((((-1.0) * npy * x1071)) + (((-1.0) * npx * x1068)) +
1905  ((sj4 * x1070)));
1906  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1907  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
1908  {
1909  continue;
1910  }
1911  }
1912 
1913  rotationfunction0(solutions);
1914  }
1915  }
1916  }
1917  }
1918  }
1919  } while (0);
1920  if (bgotonextstatement)
1921  {
1922  bool bgotonextstatement = true;
1923  do
1924  {
1925  evalcond[0] =
1926  ((-3.14159265358979) +
1927  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j5)))),
1928  6.28318530717959)));
1929  if (IKabs(evalcond[0]) < 0.0000050000000000)
1930  {
1931  bgotonextstatement = false;
1932  {
1933  IkReal j6eval[3];
1934  sj5 = 0;
1935  cj5 = -1.0;
1936  j5 = 3.14159265358979;
1937  IkReal x1072 = npx * npx;
1938  IkReal x1073 = npy * npy;
1939  IkReal x1074 = (cj4 * sj3);
1940  IkReal x1075 = (sj3 * sj4);
1941  j6eval[0] = (x1072 + x1073);
1942  j6eval[1] = ((IKabs((((npx * x1074)) + ((npy * x1075))))) +
1943  (IKabs(((((-1.0) * npy * x1074)) + ((npx * x1075))))));
1944  j6eval[2] = IKsign(((((5.0) * x1073)) + (((5.0) * x1072))));
1945  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1946  IKabs(j6eval[1]) < 0.0000010000000000 ||
1947  IKabs(j6eval[2]) < 0.0000010000000000)
1948  {
1949  {
1950  IkReal evalcond[1];
1951  bool bgotonextstatement = true;
1952  do
1953  {
1954  evalcond[0] = ((-3.14159265358979) +
1955  (IKfmod(((3.14159265358979) + (IKabs(j3))),
1956  6.28318530717959)));
1957  if (IKabs(evalcond[0]) < 0.0000050000000000)
1958  {
1959  bgotonextstatement = false;
1960  {
1961  IkReal j6eval[1];
1962  sj5 = 0;
1963  cj5 = -1.0;
1964  j5 = 3.14159265358979;
1965  sj3 = 0;
1966  cj3 = 1.0;
1967  j3 = 0;
1968  j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1969  if (IKabs(j6eval[0]) < 0.0000010000000000)
1970  {
1971  continue; // no branches [j6]
1972  }
1973  else
1974  {
1975  {
1976  IkReal j6array[2], cj6array[2], sj6array[2];
1977  bool j6valid[2] = { false };
1978  _nj6 = 2;
1979  CheckValue<IkReal> x1077 =
1980  IKatan2WithCheck(IkReal(npx),
1981  IkReal(((-1.0) * npy)),
1982  IKFAST_ATAN2_MAGTHRESH);
1983  if (!x1077.valid)
1984  {
1985  continue;
1986  }
1987  IkReal x1076 = x1077.value;
1988  j6array[0] = ((-1.0) * x1076);
1989  sj6array[0] = IKsin(j6array[0]);
1990  cj6array[0] = IKcos(j6array[0]);
1991  j6array[1] = ((3.14159265358979) + (((-1.0) * x1076)));
1992  sj6array[1] = IKsin(j6array[1]);
1993  cj6array[1] = IKcos(j6array[1]);
1994  if (j6array[0] > IKPI)
1995  {
1996  j6array[0] -= IK2PI;
1997  }
1998  else if (j6array[0] < -IKPI)
1999  {
2000  j6array[0] += IK2PI;
2001  }
2002  j6valid[0] = true;
2003  if (j6array[1] > IKPI)
2004  {
2005  j6array[1] -= IK2PI;
2006  }
2007  else if (j6array[1] < -IKPI)
2008  {
2009  j6array[1] += IK2PI;
2010  }
2011  j6valid[1] = true;
2012  for (int ij6 = 0; ij6 < 2; ++ij6)
2013  {
2014  if (!j6valid[ij6])
2015  {
2016  continue;
2017  }
2018  _ij6[0] = ij6;
2019  _ij6[1] = -1;
2020  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2021  {
2022  if (j6valid[iij6] &&
2023  IKabs(cj6array[ij6] - cj6array[iij6]) <
2024  IKFAST_SOLUTION_THRESH &&
2025  IKabs(sj6array[ij6] - sj6array[iij6]) <
2026  IKFAST_SOLUTION_THRESH)
2027  {
2028  j6valid[iij6] = false;
2029  _ij6[1] = iij6;
2030  break;
2031  }
2032  }
2033  j6 = j6array[ij6];
2034  cj6 = cj6array[ij6];
2035  sj6 = sj6array[ij6];
2036  {
2037  IkReal evalcond[1];
2038  evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
2039  (((-1.0) * npx * (IKsin(j6)))));
2040  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
2041  {
2042  continue;
2043  }
2044  }
2045 
2046  rotationfunction0(solutions);
2047  }
2048  }
2049  }
2050  }
2051  }
2052  } while (0);
2053  if (bgotonextstatement)
2054  {
2055  bool bgotonextstatement = true;
2056  do
2057  {
2058  if (1)
2059  {
2060  bgotonextstatement = false;
2061  continue; // branch miss [j6]
2062  }
2063  } while (0);
2064  if (bgotonextstatement)
2065  {
2066  }
2067  }
2068  }
2069  }
2070  else
2071  {
2072  {
2073  IkReal j6array[1], cj6array[1], sj6array[1];
2074  bool j6valid[1] = { false };
2075  _nj6 = 1;
2076  IkReal x1078 = ((2.0) * sj3);
2077  CheckValue<IkReal> x1079 = IKPowWithIntegerCheck(
2078  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2079  if (!x1079.valid)
2080  {
2081  continue;
2082  }
2083  CheckValue<IkReal> x1080 = IKatan2WithCheck(
2084  IkReal(((((-1.0) * cj4 * npy * x1078)) + ((npx * sj4 * x1078)))),
2085  IkReal((((cj4 * npx * x1078)) + ((npy * sj4 * x1078)))),
2086  IKFAST_ATAN2_MAGTHRESH);
2087  if (!x1080.valid)
2088  {
2089  continue;
2090  }
2091  j6array[0] = ((-1.5707963267949) +
2092  (((1.5707963267949) * (x1079.value))) + (x1080.value));
2093  sj6array[0] = IKsin(j6array[0]);
2094  cj6array[0] = IKcos(j6array[0]);
2095  if (j6array[0] > IKPI)
2096  {
2097  j6array[0] -= IK2PI;
2098  }
2099  else if (j6array[0] < -IKPI)
2100  {
2101  j6array[0] += IK2PI;
2102  }
2103  j6valid[0] = true;
2104  for (int ij6 = 0; ij6 < 1; ++ij6)
2105  {
2106  if (!j6valid[ij6])
2107  {
2108  continue;
2109  }
2110  _ij6[0] = ij6;
2111  _ij6[1] = -1;
2112  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2113  {
2114  if (j6valid[iij6] &&
2115  IKabs(cj6array[ij6] - cj6array[iij6]) <
2116  IKFAST_SOLUTION_THRESH &&
2117  IKabs(sj6array[ij6] - sj6array[iij6]) <
2118  IKFAST_SOLUTION_THRESH)
2119  {
2120  j6valid[iij6] = false;
2121  _ij6[1] = iij6;
2122  break;
2123  }
2124  }
2125  j6 = j6array[ij6];
2126  cj6 = cj6array[ij6];
2127  sj6 = sj6array[ij6];
2128  {
2129  IkReal evalcond[2];
2130  IkReal x1081 = IKsin(j6);
2131  IkReal x1082 = IKcos(j6);
2132  IkReal x1083 = ((0.4) * sj3);
2133  IkReal x1084 = ((1.0) * npy);
2134  evalcond[0] = ((((-1.0) * x1081 * x1084)) + ((npx * x1082)) +
2135  (((-1.0) * cj4 * x1083)));
2136  evalcond[1] = ((((-1.0) * x1082 * x1084)) + ((sj4 * x1083)) +
2137  (((-1.0) * npx * x1081)));
2138  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2139  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2140  {
2141  continue;
2142  }
2143  }
2144 
2145  rotationfunction0(solutions);
2146  }
2147  }
2148  }
2149  }
2150  }
2151  } while (0);
2152  if (bgotonextstatement)
2153  {
2154  bool bgotonextstatement = true;
2155  do
2156  {
2157  evalcond[0] =
2158  ((-3.14159265358979) +
2159  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j5)))),
2160  6.28318530717959)));
2161  if (IKabs(evalcond[0]) < 0.0000050000000000)
2162  {
2163  bgotonextstatement = false;
2164  {
2165  IkReal j6eval[3];
2166  sj5 = 1.0;
2167  cj5 = 0;
2168  j5 = 1.5707963267949;
2169  IkReal x1085 = npx * npx;
2170  IkReal x1086 = npy * npy;
2171  IkReal x1087 = ((2.0) * cj3);
2172  IkReal x1088 = ((2.0) * npx);
2173  IkReal x1089 = (sj3 * sj4);
2174  IkReal x1090 = ((2.0) * npy);
2175  j6eval[0] = (x1086 + x1085);
2176  j6eval[1] = ((IKabs((x1088 + ((x1089 * x1090)) + ((npx * x1087))))) +
2177  (IKabs(((((-1.0) * x1090)) + (((-1.0) * npy * x1087)) +
2178  ((x1088 * x1089))))));
2179  j6eval[2] = IKsign(((((5.0) * x1085)) + (((5.0) * x1086))));
2180  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2181  IKabs(j6eval[1]) < 0.0000010000000000 ||
2182  IKabs(j6eval[2]) < 0.0000010000000000)
2183  {
2184  continue; // no branches [j6]
2185  }
2186  else
2187  {
2188  {
2189  IkReal j6array[1], cj6array[1], sj6array[1];
2190  bool j6valid[1] = { false };
2191  _nj6 = 1;
2192  IkReal x1091 = ((2.0) * cj3);
2193  IkReal x1092 = ((2.0) * npx);
2194  IkReal x1093 = (sj3 * sj4);
2195  IkReal x1094 = ((2.0) * npy);
2196  CheckValue<IkReal> x1095 = IKatan2WithCheck(
2197  IkReal(((((-1.0) * x1094)) + ((x1092 * x1093)) +
2198  (((-1.0) * npy * x1091)))),
2199  IkReal((((npx * x1091)) + x1092 + ((x1093 * x1094)))),
2200  IKFAST_ATAN2_MAGTHRESH);
2201  if (!x1095.valid)
2202  {
2203  continue;
2204  }
2205  CheckValue<IkReal> x1096 = IKPowWithIntegerCheck(
2206  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2207  -1);
2208  if (!x1096.valid)
2209  {
2210  continue;
2211  }
2212  j6array[0] = ((-1.5707963267949) + (x1095.value) +
2213  (((1.5707963267949) * (x1096.value))));
2214  sj6array[0] = IKsin(j6array[0]);
2215  cj6array[0] = IKcos(j6array[0]);
2216  if (j6array[0] > IKPI)
2217  {
2218  j6array[0] -= IK2PI;
2219  }
2220  else if (j6array[0] < -IKPI)
2221  {
2222  j6array[0] += IK2PI;
2223  }
2224  j6valid[0] = true;
2225  for (int ij6 = 0; ij6 < 1; ++ij6)
2226  {
2227  if (!j6valid[ij6])
2228  {
2229  continue;
2230  }
2231  _ij6[0] = ij6;
2232  _ij6[1] = -1;
2233  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2234  {
2235  if (j6valid[iij6] &&
2236  IKabs(cj6array[ij6] - cj6array[iij6]) <
2237  IKFAST_SOLUTION_THRESH &&
2238  IKabs(sj6array[ij6] - sj6array[iij6]) <
2239  IKFAST_SOLUTION_THRESH)
2240  {
2241  j6valid[iij6] = false;
2242  _ij6[1] = iij6;
2243  break;
2244  }
2245  }
2246  j6 = j6array[ij6];
2247  cj6 = cj6array[ij6];
2248  sj6 = sj6array[ij6];
2249  {
2250  IkReal evalcond[2];
2251  IkReal x1097 = IKsin(j6);
2252  IkReal x1098 = IKcos(j6);
2253  IkReal x1099 = ((1.0) * npx);
2254  evalcond[0] = ((0.4) + (((0.4) * cj3)) + ((npy * x1097)) +
2255  (((-1.0) * x1098 * x1099)));
2256  evalcond[1] =
2257  ((((-1.0) * x1097 * x1099)) + (((-1.0) * npy * x1098)) +
2258  (((0.4) * sj3 * sj4)));
2259  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2260  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2261  {
2262  continue;
2263  }
2264  }
2265 
2266  rotationfunction0(solutions);
2267  }
2268  }
2269  }
2270  }
2271  }
2272  } while (0);
2273  if (bgotonextstatement)
2274  {
2275  bool bgotonextstatement = true;
2276  do
2277  {
2278  evalcond[0] =
2279  ((-3.14159265358979) +
2280  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j5)))),
2281  6.28318530717959)));
2282  if (IKabs(evalcond[0]) < 0.0000050000000000)
2283  {
2284  bgotonextstatement = false;
2285  {
2286  IkReal j6eval[3];
2287  sj5 = -1.0;
2288  cj5 = 0;
2289  j5 = -1.5707963267949;
2290  IkReal x1100 = npx * npx;
2291  IkReal x1101 = npy * npy;
2292  IkReal x1102 = ((2.0) * cj3);
2293  IkReal x1103 = ((2.0) * npx);
2294  IkReal x1104 = (sj3 * sj4);
2295  IkReal x1105 = ((2.0) * npy);
2296  j6eval[0] = (x1100 + x1101);
2297  j6eval[1] = ((IKabs(((((-1.0) * npx * x1102)) + ((x1104 * x1105)) +
2298  (((-1.0) * x1103))))) +
2299  (IKabs((x1105 + ((npy * x1102)) + ((x1103 * x1104))))));
2300  j6eval[2] = IKsign(((((5.0) * x1101)) + (((5.0) * x1100))));
2301  if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2302  IKabs(j6eval[1]) < 0.0000010000000000 ||
2303  IKabs(j6eval[2]) < 0.0000010000000000)
2304  {
2305  continue; // no branches [j6]
2306  }
2307  else
2308  {
2309  {
2310  IkReal j6array[1], cj6array[1], sj6array[1];
2311  bool j6valid[1] = { false };
2312  _nj6 = 1;
2313  IkReal x1106 = ((2.0) * cj3);
2314  IkReal x1107 = ((2.0) * npx);
2315  IkReal x1108 = (sj3 * sj4);
2316  IkReal x1109 = ((2.0) * npy);
2317  CheckValue<IkReal> x1110 = IKPowWithIntegerCheck(
2318  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2319  -1);
2320  if (!x1110.valid)
2321  {
2322  continue;
2323  }
2324  CheckValue<IkReal> x1111 = IKatan2WithCheck(
2325  IkReal((x1109 + ((npy * x1106)) + ((x1107 * x1108)))),
2326  IkReal((((x1108 * x1109)) + (((-1.0) * npx * x1106)) +
2327  (((-1.0) * x1107)))),
2328  IKFAST_ATAN2_MAGTHRESH);
2329  if (!x1111.valid)
2330  {
2331  continue;
2332  }
2333  j6array[0] =
2334  ((-1.5707963267949) + (((1.5707963267949) * (x1110.value))) +
2335  (x1111.value));
2336  sj6array[0] = IKsin(j6array[0]);
2337  cj6array[0] = IKcos(j6array[0]);
2338  if (j6array[0] > IKPI)
2339  {
2340  j6array[0] -= IK2PI;
2341  }
2342  else if (j6array[0] < -IKPI)
2343  {
2344  j6array[0] += IK2PI;
2345  }
2346  j6valid[0] = true;
2347  for (int ij6 = 0; ij6 < 1; ++ij6)
2348  {
2349  if (!j6valid[ij6])
2350  {
2351  continue;
2352  }
2353  _ij6[0] = ij6;
2354  _ij6[1] = -1;
2355  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2356  {
2357  if (j6valid[iij6] &&
2358  IKabs(cj6array[ij6] - cj6array[iij6]) <
2359  IKFAST_SOLUTION_THRESH &&
2360  IKabs(sj6array[ij6] - sj6array[iij6]) <
2361  IKFAST_SOLUTION_THRESH)
2362  {
2363  j6valid[iij6] = false;
2364  _ij6[1] = iij6;
2365  break;
2366  }
2367  }
2368  j6 = j6array[ij6];
2369  cj6 = cj6array[ij6];
2370  sj6 = sj6array[ij6];
2371  {
2372  IkReal evalcond[2];
2373  IkReal x1112 = IKsin(j6);
2374  IkReal x1113 = IKcos(j6);
2375  IkReal x1114 = ((1.0) * x1112);
2376  evalcond[0] = ((0.4) + ((npx * x1113)) +
2377  (((-1.0) * npy * x1114)) + (((0.4) * cj3)));
2378  evalcond[1] =
2379  ((((-1.0) * npy * x1113)) + (((-1.0) * npx * x1114)) +
2380  (((0.4) * sj3 * sj4)));
2381  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2382  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2383  {
2384  continue;
2385  }
2386  }
2387 
2388  rotationfunction0(solutions);
2389  }
2390  }
2391  }
2392  }
2393  }
2394  } while (0);
2395  if (bgotonextstatement)
2396  {
2397  bool bgotonextstatement = true;
2398  do
2399  {
2400  if (1)
2401  {
2402  bgotonextstatement = false;
2403  continue; // branch miss [j6]
2404  }
2405  } while (0);
2406  if (bgotonextstatement)
2407  {
2408  }
2409  }
2410  }
2411  }
2412  }
2413  }
2414  }
2415  }
2416  else
2417  {
2418  {
2419  IkReal j6array[1], cj6array[1], sj6array[1];
2420  bool j6valid[1] = { false };
2421  _nj6 = 1;
2422  IkReal x1115 = ((2.0) * sj5);
2423  IkReal x1116 = ((2.0) * sj3);
2424  IkReal x1117 = (cj4 * cj5);
2425  CheckValue<IkReal> x1118 = IKatan2WithCheck(
2426  IkReal((((npy * x1116 * x1117)) + (((-1.0) * npy * x1115)) +
2427  ((npx * sj4 * x1116)) + (((-1.0) * cj3 * npy * x1115)))),
2428  IkReal((((npx * x1115)) + (((-1.0) * npx * x1116 * x1117)) +
2429  ((npy * sj4 * x1116)) + ((cj3 * npx * x1115)))),
2430  IKFAST_ATAN2_MAGTHRESH);
2431  if (!x1118.valid)
2432  {
2433  continue;
2434  }
2435  CheckValue<IkReal> x1119 = IKPowWithIntegerCheck(
2436  IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2437  if (!x1119.valid)
2438  {
2439  continue;
2440  }
2441  j6array[0] =
2442  ((-1.5707963267949) + (x1118.value) + (((1.5707963267949) * (x1119.value))));
2443  sj6array[0] = IKsin(j6array[0]);
2444  cj6array[0] = IKcos(j6array[0]);
2445  if (j6array[0] > IKPI)
2446  {
2447  j6array[0] -= IK2PI;
2448  }
2449  else if (j6array[0] < -IKPI)
2450  {
2451  j6array[0] += IK2PI;
2452  }
2453  j6valid[0] = true;
2454  for (int ij6 = 0; ij6 < 1; ++ij6)
2455  {
2456  if (!j6valid[ij6])
2457  {
2458  continue;
2459  }
2460  _ij6[0] = ij6;
2461  _ij6[1] = -1;
2462  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2463  {
2464  if (j6valid[iij6] &&
2465  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2466  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2467  {
2468  j6valid[iij6] = false;
2469  _ij6[1] = iij6;
2470  break;
2471  }
2472  }
2473  j6 = j6array[ij6];
2474  cj6 = cj6array[ij6];
2475  sj6 = sj6array[ij6];
2476  {
2477  IkReal evalcond[4];
2478  IkReal x1120 = IKsin(j6);
2479  IkReal x1121 = IKcos(j6);
2480  IkReal x1122 = ((0.4) * sj3);
2481  IkReal x1123 = ((1.0) * npx);
2482  IkReal x1124 = ((0.4) * sj5);
2483  IkReal x1125 = (npy * x1120);
2484  evalcond[0] =
2485  ((((-1.0) * npy * x1121)) + ((sj4 * x1122)) + (((-1.0) * x1120 * x1123)));
2486  evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1122)) +
2487  (((-1.0) * cj5 * x1121 * x1123)) + ((cj5 * x1125)));
2488  evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * sj5 * x1121 * x1123)) +
2489  (((-1.0) * cj5 * npz)) + ((sj5 * x1125)));
2490  evalcond[3] = ((((-1.0) * cj4 * cj5 * x1122)) + x1124 + x1125 +
2491  (((-1.0) * x1121 * x1123)) + ((cj3 * x1124)));
2492  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2493  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2494  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2495  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2496  {
2497  continue;
2498  }
2499  }
2500 
2501  rotationfunction0(solutions);
2502  }
2503  }
2504  }
2505  }
2506  }
2507  else
2508  {
2509  {
2510  IkReal j6array[1], cj6array[1], sj6array[1];
2511  bool j6valid[1] = { false };
2512  _nj6 = 1;
2513  IkReal x1126 = ((2.0) * cj3);
2514  IkReal x1127 = ((5.0) * sj5);
2515  IkReal x1128 = ((2.0) * npx);
2516  IkReal x1129 = ((2.0) * npy);
2517  IkReal x1130 = ((5.0) * cj5 * npz);
2518  IkReal x1131 = (sj3 * sj4 * sj5);
2519  CheckValue<IkReal> x1132 = IKPowWithIntegerCheck(
2520  IKsign((((x1127 * (npy * npy))) + ((x1127 * (npx * npx))))), -1);
2521  if (!x1132.valid)
2522  {
2523  continue;
2524  }
2525  CheckValue<IkReal> x1133 = IKatan2WithCheck(
2526  IkReal(((((-1.0) * npy * x1126)) + ((npy * x1130)) + (((-1.0) * x1129)) +
2527  ((x1128 * x1131)))),
2528  IkReal((x1128 + ((npx * x1126)) + (((-1.0) * npx * x1130)) + ((x1129 * x1131)))),
2529  IKFAST_ATAN2_MAGTHRESH);
2530  if (!x1133.valid)
2531  {
2532  continue;
2533  }
2534  j6array[0] =
2535  ((-1.5707963267949) + (((1.5707963267949) * (x1132.value))) + (x1133.value));
2536  sj6array[0] = IKsin(j6array[0]);
2537  cj6array[0] = IKcos(j6array[0]);
2538  if (j6array[0] > IKPI)
2539  {
2540  j6array[0] -= IK2PI;
2541  }
2542  else if (j6array[0] < -IKPI)
2543  {
2544  j6array[0] += IK2PI;
2545  }
2546  j6valid[0] = true;
2547  for (int ij6 = 0; ij6 < 1; ++ij6)
2548  {
2549  if (!j6valid[ij6])
2550  {
2551  continue;
2552  }
2553  _ij6[0] = ij6;
2554  _ij6[1] = -1;
2555  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2556  {
2557  if (j6valid[iij6] &&
2558  IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2559  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2560  {
2561  j6valid[iij6] = false;
2562  _ij6[1] = iij6;
2563  break;
2564  }
2565  }
2566  j6 = j6array[ij6];
2567  cj6 = cj6array[ij6];
2568  sj6 = sj6array[ij6];
2569  {
2570  IkReal evalcond[4];
2571  IkReal x1134 = IKsin(j6);
2572  IkReal x1135 = IKcos(j6);
2573  IkReal x1136 = ((0.4) * sj3);
2574  IkReal x1137 = ((1.0) * npx);
2575  IkReal x1138 = ((0.4) * sj5);
2576  IkReal x1139 = (npy * x1134);
2577  evalcond[0] =
2578  ((((-1.0) * npy * x1135)) + ((sj4 * x1136)) + (((-1.0) * x1134 * x1137)));
2579  evalcond[1] = ((((-1.0) * cj4 * x1136)) + ((npz * sj5)) +
2580  (((-1.0) * cj5 * x1135 * x1137)) + ((cj5 * x1139)));
2581  evalcond[2] = ((0.4) + (((-1.0) * sj5 * x1135 * x1137)) + (((0.4) * cj3)) +
2582  (((-1.0) * cj5 * npz)) + ((sj5 * x1139)));
2583  evalcond[3] = (x1139 + x1138 + ((cj3 * x1138)) + (((-1.0) * cj4 * cj5 * x1136)) +
2584  (((-1.0) * x1135 * x1137)));
2585  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2586  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2587  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2588  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2589  {
2590  continue;
2591  }
2592  }
2593 
2594  rotationfunction0(solutions);
2595  }
2596  }
2597  }
2598  }
2599  }
2600  else
2601  {
2602  {
2603  IkReal j6array[1], cj6array[1], sj6array[1];
2604  bool j6valid[1] = { false };
2605  _nj6 = 1;
2606  IkReal x1140 = (cj5 * sj4);
2607  IkReal x1141 = ((5.0) * cj5);
2608  IkReal x1142 = ((5.0) * npz * sj5);
2609  IkReal x1143 = ((2.0) * npy * sj3);
2610  IkReal x1144 = ((2.0) * npx * sj3);
2611  CheckValue<IkReal> x1145 = IKatan2WithCheck(
2612  IkReal((((x1140 * x1144)) + (((-1.0) * npy * x1142)) + ((cj4 * x1143)))),
2613  IkReal((((x1140 * x1143)) + (((-1.0) * cj4 * x1144)) + ((npx * x1142)))),
2614  IKFAST_ATAN2_MAGTHRESH);
2615  if (!x1145.valid)
2616  {
2617  continue;
2618  }
2619  CheckValue<IkReal> x1146 =
2620  IKPowWithIntegerCheck(IKsign((((x1141 * (npx * npx))) + ((x1141 * (npy * npy))))), -1);
2621  if (!x1146.valid)
2622  {
2623  continue;
2624  }
2625  j6array[0] = ((-1.5707963267949) + (x1145.value) + (((1.5707963267949) * (x1146.value))));
2626  sj6array[0] = IKsin(j6array[0]);
2627  cj6array[0] = IKcos(j6array[0]);
2628  if (j6array[0] > IKPI)
2629  {
2630  j6array[0] -= IK2PI;
2631  }
2632  else if (j6array[0] < -IKPI)
2633  {
2634  j6array[0] += IK2PI;
2635  }
2636  j6valid[0] = true;
2637  for (int ij6 = 0; ij6 < 1; ++ij6)
2638  {
2639  if (!j6valid[ij6])
2640  {
2641  continue;
2642  }
2643  _ij6[0] = ij6;
2644  _ij6[1] = -1;
2645  for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2646  {
2647  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2648  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2649  {
2650  j6valid[iij6] = false;
2651  _ij6[1] = iij6;
2652  break;
2653  }
2654  }
2655  j6 = j6array[ij6];
2656  cj6 = cj6array[ij6];
2657  sj6 = sj6array[ij6];
2658  {
2659  IkReal evalcond[4];
2660  IkReal x1147 = IKsin(j6);
2661  IkReal x1148 = IKcos(j6);
2662  IkReal x1149 = ((0.4) * sj3);
2663  IkReal x1150 = ((1.0) * npx);
2664  IkReal x1151 = ((0.4) * sj5);
2665  IkReal x1152 = (npy * x1147);
2666  evalcond[0] = (((sj4 * x1149)) + (((-1.0) * x1147 * x1150)) + (((-1.0) * npy * x1148)));
2667  evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1149)) +
2668  (((-1.0) * cj5 * x1148 * x1150)) + ((cj5 * x1152)));
2669  evalcond[2] = ((0.4) + (((0.4) * cj3)) + ((sj5 * x1152)) + (((-1.0) * cj5 * npz)) +
2670  (((-1.0) * sj5 * x1148 * x1150)));
2671  evalcond[3] = (x1151 + x1152 + ((cj3 * x1151)) + (((-1.0) * x1148 * x1150)) +
2672  (((-1.0) * cj4 * cj5 * x1149)));
2673  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2674  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2675  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2676  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2677  {
2678  continue;
2679  }
2680  }
2681 
2682  rotationfunction0(solutions);
2683  }
2684  }
2685  }
2686  }
2687  }
2688  }
2689  }
2690  }
2691  }
2692  else
2693  {
2694  {
2695  IkReal j6array[2], cj6array[2], sj6array[2];
2696  bool j6valid[2] = { false };
2697  _nj6 = 2;
2698  CheckValue<IkReal> x1155 =
2699  IKatan2WithCheck(IkReal(((-1.0) * npy)), IkReal(((-1.0) * npx)), IKFAST_ATAN2_MAGTHRESH);
2700  if (!x1155.valid)
2701  {
2702  continue;
2703  }
2704  IkReal x1153 = ((1.0) * (x1155.value));
2705  if ((((npx * npx) + (npy * npy))) < -0.00001)
2706  continue;
2707  CheckValue<IkReal> x1156 = IKPowWithIntegerCheck(IKabs(IKsqrt(((npx * npx) + (npy * npy)))), -1);
2708  if (!x1156.valid)
2709  {
2710  continue;
2711  }
2712  if ((((0.4) * sj3 * sj4 * (x1156.value))) < -1 - IKFAST_SINCOS_THRESH ||
2713  (((0.4) * sj3 * sj4 * (x1156.value))) > 1 + IKFAST_SINCOS_THRESH)
2714  continue;
2715  IkReal x1154 = IKasin(((0.4) * sj3 * sj4 * (x1156.value)));
2716  j6array[0] = ((((-1.0) * x1153)) + (((-1.0) * x1154)));
2717  sj6array[0] = IKsin(j6array[0]);
2718  cj6array[0] = IKcos(j6array[0]);
2719  j6array[1] = ((3.14159265358979) + x1154 + (((-1.0) * x1153)));
2720  sj6array[1] = IKsin(j6array[1]);
2721  cj6array[1] = IKcos(j6array[1]);
2722  if (j6array[0] > IKPI)
2723  {
2724  j6array[0] -= IK2PI;
2725  }
2726  else if (j6array[0] < -IKPI)
2727  {
2728  j6array[0] += IK2PI;
2729  }
2730  j6valid[0] = true;
2731  if (j6array[1] > IKPI)
2732  {
2733  j6array[1] -= IK2PI;
2734  }
2735  else if (j6array[1] < -IKPI)
2736  {
2737  j6array[1] += IK2PI;
2738  }
2739  j6valid[1] = true;
2740  for (int ij6 = 0; ij6 < 2; ++ij6)
2741  {
2742  if (!j6valid[ij6])
2743  {
2744  continue;
2745  }
2746  _ij6[0] = ij6;
2747  _ij6[1] = -1;
2748  for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2749  {
2750  if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2751  IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2752  {
2753  j6valid[iij6] = false;
2754  _ij6[1] = iij6;
2755  break;
2756  }
2757  }
2758  j6 = j6array[ij6];
2759  cj6 = cj6array[ij6];
2760  sj6 = sj6array[ij6];
2761 
2762  {
2763  IkReal j5eval[3];
2764  IkReal x1157 = (cj6 * npx);
2765  IkReal x1158 = (npy * sj6);
2766  IkReal x1159 = ((2.0) * cj3);
2767  IkReal x1160 = ((5.0) * npz);
2768  IkReal x1161 = ((0.8) * cj4 * sj3);
2769  IkReal x1162 = (cj4 * npz * sj3);
2770  j5eval[0] = (x1158 + x1162 + ((cj3 * x1158)) + (((-1.0) * cj3 * x1157)) + (((-1.0) * x1157)));
2771  j5eval[1] = ((IKabs(((-0.8) + (((-1.6) * cj3)) + ((npz * x1160)) + (((-0.8) * (cj3 * cj3)))))) +
2772  (IKabs((x1161 + ((cj3 * x1161)) + ((x1158 * x1160)) + (((-1.0) * x1157 * x1160))))));
2773  j5eval[2] = IKsign(((((-1.0) * x1157 * x1159)) + ((x1158 * x1159)) + (((2.0) * x1158)) +
2774  (((2.0) * x1162)) + (((-2.0) * x1157))));
2775  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2776  IKabs(j5eval[2]) < 0.0000010000000000)
2777  {
2778  {
2779  IkReal j5eval[3];
2780  IkReal x1163 = cj3 * cj3;
2781  IkReal x1164 = cj4 * cj4;
2782  IkReal x1165 = ((10.0) * cj3);
2783  IkReal x1166 = (npy * sj6);
2784  IkReal x1167 = (cj6 * npx);
2785  IkReal x1168 = ((4.0) * x1164);
2786  IkReal x1169 = ((10.0) * cj4 * sj3);
2787  j5eval[0] = ((1.0) + x1164 + x1163 + (((-1.0) * x1163 * x1164)) + (((2.0) * cj3)));
2788  j5eval[1] =
2789  IKsign(((4.0) + x1168 + (((8.0) * cj3)) + (((-1.0) * x1163 * x1168)) + (((4.0) * x1163))));
2790  j5eval[2] = ((IKabs((((x1165 * x1167)) + ((npz * x1169)) + (((10.0) * x1167)) +
2791  (((-10.0) * x1166)) + (((-1.0) * x1165 * x1166))))) +
2792  (IKabs((((x1166 * x1169)) + (((10.0) * npz)) + ((npz * x1165)) +
2793  (((-1.0) * x1167 * x1169))))));
2794  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2795  IKabs(j5eval[2]) < 0.0000010000000000)
2796  {
2797  {
2798  IkReal j5eval[3];
2799  IkReal x1170 = cj6 * cj6;
2800  IkReal x1171 = npy * npy;
2801  IkReal x1172 = npx * npx;
2802  IkReal x1173 = npz * npz;
2803  IkReal x1174 = ((2.0) * cj3);
2804  IkReal x1175 = (cj6 * npx);
2805  IkReal x1176 = ((2.0) * npz);
2806  IkReal x1177 = (cj4 * sj3);
2807  IkReal x1178 = ((2.0) * npy * sj6);
2808  IkReal x1179 = (x1170 * x1171);
2809  IkReal x1180 = (x1170 * x1172);
2810  j5eval[0] = (x1180 + x1173 + x1171 + (((-1.0) * x1175 * x1178)) + (((-1.0) * x1179)));
2811  j5eval[1] =
2812  ((IKabs((((x1174 * x1175)) + ((x1176 * x1177)) + (((2.0) * x1175)) +
2813  (((-1.0) * npy * sj6 * x1174)) + (((-1.0) * x1178))))) +
2814  (IKabs((x1176 + ((x1177 * x1178)) + ((npz * x1174)) + (((-2.0) * x1175 * x1177))))));
2815  j5eval[2] = IKsign(((((5.0) * x1180)) + (((-5.0) * x1179)) + (((5.0) * x1171)) +
2816  (((5.0) * x1173)) + (((-10.0) * npy * sj6 * x1175))));
2817  if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2818  IKabs(j5eval[2]) < 0.0000010000000000)
2819  {
2820  {
2821  IkReal evalcond[2];
2822  bool bgotonextstatement = true;
2823  do
2824  {
2825  evalcond[0] = ((-3.14159265358979) +
2826  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
2827  6.28318530717959)));
2828  evalcond[1] = pp;
2829  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
2830  IKabs(evalcond[1]) < 0.0000050000000000)
2831  {
2832  bgotonextstatement = false;
2833  {
2834  IkReal j5eval[1];
2835  sj3 = 0;
2836  cj3 = -1.0;
2837  j3 = 3.14159265358979;
2838  j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
2839  if (IKabs(j5eval[0]) < 0.0000000100000000)
2840  {
2841  continue; // no branches [j5]
2842  }
2843  else
2844  {
2845  IkReal op[2 + 1], zeror[2];
2846  int numroots;
2847  IkReal x1181 = (npy * sj6);
2848  IkReal x1182 = (cj6 * npx);
2849  op[0] = (x1182 + (((-1.0) * x1181)));
2850  op[1] = 0;
2851  op[2] = (x1181 + (((-1.0) * x1182)));
2852  polyroots2(op, zeror, numroots);
2853  IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
2854  int numsolutions = 0;
2855  for (int ij5 = 0; ij5 < numroots; ++ij5)
2856  {
2857  IkReal htj5 = zeror[ij5];
2858  tempj5array[0] = ((2.0) * (atan(htj5)));
2859  for (int kj5 = 0; kj5 < 1; ++kj5)
2860  {
2861  j5array[numsolutions] = tempj5array[kj5];
2862  if (j5array[numsolutions] > IKPI)
2863  {
2864  j5array[numsolutions] -= IK2PI;
2865  }
2866  else if (j5array[numsolutions] < -IKPI)
2867  {
2868  j5array[numsolutions] += IK2PI;
2869  }
2870  sj5array[numsolutions] = IKsin(j5array[numsolutions]);
2871  cj5array[numsolutions] = IKcos(j5array[numsolutions]);
2872  numsolutions++;
2873  }
2874  }
2875  bool j5valid[2] = { true, true };
2876  _nj5 = 2;
2877  for (int ij5 = 0; ij5 < numsolutions; ++ij5)
2878  {
2879  if (!j5valid[ij5])
2880  {
2881  continue;
2882  }
2883  j5 = j5array[ij5];
2884  cj5 = cj5array[ij5];
2885  sj5 = sj5array[ij5];
2886  htj5 = IKtan(j5 / 2);
2887 
2888  _ij5[0] = ij5;
2889  _ij5[1] = -1;
2890  for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
2891  {
2892  if (j5valid[iij5] &&
2893  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2894  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2895  {
2896  j5valid[iij5] = false;
2897  _ij5[1] = iij5;
2898  break;
2899  }
2900  }
2901  rotationfunction0(solutions);
2902  }
2903  }
2904  }
2905  }
2906  } while (0);
2907  if (bgotonextstatement)
2908  {
2909  bool bgotonextstatement = true;
2910  do
2911  {
2912  if (1)
2913  {
2914  bgotonextstatement = false;
2915  continue; // branch miss [j5]
2916  }
2917  } while (0);
2918  if (bgotonextstatement)
2919  {
2920  }
2921  }
2922  }
2923  }
2924  else
2925  {
2926  {
2927  IkReal j5array[1], cj5array[1], sj5array[1];
2928  bool j5valid[1] = { false };
2929  _nj5 = 1;
2930  IkReal x1183 = cj6 * cj6;
2931  IkReal x1184 = npy * npy;
2932  IkReal x1185 = (npy * sj6);
2933  IkReal x1186 = ((2.0) * cj3);
2934  IkReal x1187 = (cj6 * npx);
2935  IkReal x1188 = ((2.0) * npz);
2936  IkReal x1189 = (cj4 * sj3);
2937  IkReal x1190 = ((5.0) * x1184);
2938  CheckValue<IkReal> x1191 = IKPowWithIntegerCheck(
2939  IKsign((x1190 + (((-10.0) * x1185 * x1187)) + (((5.0) * (npz * npz))) +
2940  (((-1.0) * x1183 * x1190)) + (((5.0) * x1183 * (npx * npx))))),
2941  -1);
2942  if (!x1191.valid)
2943  {
2944  continue;
2945  }
2946  CheckValue<IkReal> x1192 =
2947  IKatan2WithCheck(IkReal((((x1186 * x1187)) + ((x1188 * x1189)) + (((2.0) * x1187)) +
2948  (((-2.0) * x1185)) + (((-1.0) * x1185 * x1186)))),
2949  IkReal(((((2.0) * x1185 * x1189)) + x1188 +
2950  (((-2.0) * x1187 * x1189)) + ((npz * x1186)))),
2951  IKFAST_ATAN2_MAGTHRESH);
2952  if (!x1192.valid)
2953  {
2954  continue;
2955  }
2956  j5array[0] =
2957  ((-1.5707963267949) + (((1.5707963267949) * (x1191.value))) + (x1192.value));
2958  sj5array[0] = IKsin(j5array[0]);
2959  cj5array[0] = IKcos(j5array[0]);
2960  if (j5array[0] > IKPI)
2961  {
2962  j5array[0] -= IK2PI;
2963  }
2964  else if (j5array[0] < -IKPI)
2965  {
2966  j5array[0] += IK2PI;
2967  }
2968  j5valid[0] = true;
2969  for (int ij5 = 0; ij5 < 1; ++ij5)
2970  {
2971  if (!j5valid[ij5])
2972  {
2973  continue;
2974  }
2975  _ij5[0] = ij5;
2976  _ij5[1] = -1;
2977  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2978  {
2979  if (j5valid[iij5] &&
2980  IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2981  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2982  {
2983  j5valid[iij5] = false;
2984  _ij5[1] = iij5;
2985  break;
2986  }
2987  }
2988  j5 = j5array[ij5];
2989  cj5 = cj5array[ij5];
2990  sj5 = sj5array[ij5];
2991  {
2992  IkReal evalcond[4];
2993  IkReal x1193 = IKcos(j5);
2994  IkReal x1194 = IKsin(j5);
2995  IkReal x1195 = (npy * sj6);
2996  IkReal x1196 = ((1.0) * npz);
2997  IkReal x1197 = ((1.0) * cj6 * npx);
2998  IkReal x1198 = ((0.4) * cj4 * sj3);
2999  IkReal x1199 = ((0.4) * x1194);
3000  IkReal x1200 = ((0.4) * x1193);
3001  evalcond[0] = (x1200 + ((x1194 * x1198)) + (((-1.0) * x1196)) + ((cj3 * x1200)));
3002  evalcond[1] = ((((-1.0) * x1193 * x1197)) + (((-1.0) * x1198)) + ((npz * x1194)) +
3003  ((x1193 * x1195)));
3004  evalcond[2] = ((0.4) + (((-1.0) * x1193 * x1196)) + ((x1194 * x1195)) +
3005  (((-1.0) * x1194 * x1197)) + (((0.4) * cj3)));
3006  evalcond[3] = (x1199 + x1195 + (((-1.0) * x1193 * x1198)) + (((-1.0) * x1197)) +
3007  ((cj3 * x1199)));
3008  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3009  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3010  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3011  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3012  {
3013  continue;
3014  }
3015  }
3016 
3017  rotationfunction0(solutions);
3018  }
3019  }
3020  }
3021  }
3022  }
3023  else
3024  {
3025  {
3026  IkReal j5array[1], cj5array[1], sj5array[1];
3027  bool j5valid[1] = { false };
3028  _nj5 = 1;
3029  IkReal x1201 = cj3 * cj3;
3030  IkReal x1202 = cj4 * cj4;
3031  IkReal x1203 = ((10.0) * cj3);
3032  IkReal x1204 = (npy * sj6);
3033  IkReal x1205 = (cj6 * npx);
3034  IkReal x1206 = ((10.0) * cj4 * sj3);
3035  IkReal x1207 = ((4.0) * x1202);
3036  CheckValue<IkReal> x1208 =
3037  IKPowWithIntegerCheck(IKsign(((4.0) + x1207 + (((8.0) * cj3)) + (((4.0) * x1201)) +
3038  (((-1.0) * x1201 * x1207)))),
3039  -1);
3040  if (!x1208.valid)
3041  {
3042  continue;
3043  }
3044  CheckValue<IkReal> x1209 =
3045  IKatan2WithCheck(IkReal(((((-10.0) * x1204)) + (((10.0) * x1205)) + ((npz * x1206)) +
3046  ((x1203 * x1205)) + (((-1.0) * x1203 * x1204)))),
3047  IkReal((((x1204 * x1206)) + (((10.0) * npz)) + ((npz * x1203)) +
3048  (((-1.0) * x1205 * x1206)))),
3049  IKFAST_ATAN2_MAGTHRESH);
3050  if (!x1209.valid)
3051  {
3052  continue;
3053  }
3054  j5array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x1208.value))) + (x1209.value));
3055  sj5array[0] = IKsin(j5array[0]);
3056  cj5array[0] = IKcos(j5array[0]);
3057  if (j5array[0] > IKPI)
3058  {
3059  j5array[0] -= IK2PI;
3060  }
3061  else if (j5array[0] < -IKPI)
3062  {
3063  j5array[0] += IK2PI;
3064  }
3065  j5valid[0] = true;
3066  for (int ij5 = 0; ij5 < 1; ++ij5)
3067  {
3068  if (!j5valid[ij5])
3069  {
3070  continue;
3071  }
3072  _ij5[0] = ij5;
3073  _ij5[1] = -1;
3074  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3075  {
3076  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3077  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3078  {
3079  j5valid[iij5] = false;
3080  _ij5[1] = iij5;
3081  break;
3082  }
3083  }
3084  j5 = j5array[ij5];
3085  cj5 = cj5array[ij5];
3086  sj5 = sj5array[ij5];
3087  {
3088  IkReal evalcond[4];
3089  IkReal x1210 = IKcos(j5);
3090  IkReal x1211 = IKsin(j5);
3091  IkReal x1212 = (npy * sj6);
3092  IkReal x1213 = ((1.0) * npz);
3093  IkReal x1214 = ((1.0) * cj6 * npx);
3094  IkReal x1215 = ((0.4) * cj4 * sj3);
3095  IkReal x1216 = ((0.4) * x1211);
3096  IkReal x1217 = ((0.4) * x1210);
3097  evalcond[0] = (x1217 + ((cj3 * x1217)) + ((x1211 * x1215)) + (((-1.0) * x1213)));
3098  evalcond[1] = (((x1210 * x1212)) + ((npz * x1211)) + (((-1.0) * x1210 * x1214)) +
3099  (((-1.0) * x1215)));
3100  evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * x1210 * x1213)) +
3101  (((-1.0) * x1211 * x1214)) + ((x1211 * x1212)));
3102  evalcond[3] =
3103  (x1212 + x1216 + (((-1.0) * x1210 * x1215)) + ((cj3 * x1216)) + (((-1.0) * x1214)));
3104  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3105  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3106  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3107  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3108  {
3109  continue;
3110  }
3111  }
3112 
3113  rotationfunction0(solutions);
3114  }
3115  }
3116  }
3117  }
3118  }
3119  else
3120  {
3121  {
3122  IkReal j5array[1], cj5array[1], sj5array[1];
3123  bool j5valid[1] = { false };
3124  _nj5 = 1;
3125  IkReal x1218 = (npy * sj6);
3126  IkReal x1219 = (cj6 * npx);
3127  IkReal x1220 = ((2.0) * cj3);
3128  IkReal x1221 = ((5.0) * npz);
3129  IkReal x1222 = ((0.8) * cj4 * sj3);
3130  CheckValue<IkReal> x1223 = IKatan2WithCheck(
3131  IkReal(((-0.8) + ((npz * x1221)) + (((-1.6) * cj3)) + (((-0.8) * (cj3 * cj3))))),
3132  IkReal((x1222 + ((cj3 * x1222)) + ((x1218 * x1221)) + (((-1.0) * x1219 * x1221)))),
3133  IKFAST_ATAN2_MAGTHRESH);
3134  if (!x1223.valid)
3135  {
3136  continue;
3137  }
3138  CheckValue<IkReal> x1224 = IKPowWithIntegerCheck(
3139  IKsign((((x1218 * x1220)) + (((2.0) * x1218)) + (((-1.0) * x1219 * x1220)) +
3140  (((-2.0) * x1219)) + (((2.0) * cj4 * npz * sj3)))),
3141  -1);
3142  if (!x1224.valid)
3143  {
3144  continue;
3145  }
3146  j5array[0] = ((-1.5707963267949) + (x1223.value) + (((1.5707963267949) * (x1224.value))));
3147  sj5array[0] = IKsin(j5array[0]);
3148  cj5array[0] = IKcos(j5array[0]);
3149  if (j5array[0] > IKPI)
3150  {
3151  j5array[0] -= IK2PI;
3152  }
3153  else if (j5array[0] < -IKPI)
3154  {
3155  j5array[0] += IK2PI;
3156  }
3157  j5valid[0] = true;
3158  for (int ij5 = 0; ij5 < 1; ++ij5)
3159  {
3160  if (!j5valid[ij5])
3161  {
3162  continue;
3163  }
3164  _ij5[0] = ij5;
3165  _ij5[1] = -1;
3166  for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3167  {
3168  if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3169  IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3170  {
3171  j5valid[iij5] = false;
3172  _ij5[1] = iij5;
3173  break;
3174  }
3175  }
3176  j5 = j5array[ij5];
3177  cj5 = cj5array[ij5];
3178  sj5 = sj5array[ij5];
3179  {
3180  IkReal evalcond[4];
3181  IkReal x1225 = IKcos(j5);
3182  IkReal x1226 = IKsin(j5);
3183  IkReal x1227 = (npy * sj6);
3184  IkReal x1228 = ((1.0) * npz);
3185  IkReal x1229 = ((1.0) * cj6 * npx);
3186  IkReal x1230 = ((0.4) * cj4 * sj3);
3187  IkReal x1231 = ((0.4) * x1226);
3188  IkReal x1232 = ((0.4) * x1225);
3189  evalcond[0] = (((cj3 * x1232)) + x1232 + (((-1.0) * x1228)) + ((x1226 * x1230)));
3190  evalcond[1] =
3191  (((npz * x1226)) + (((-1.0) * x1225 * x1229)) + (((-1.0) * x1230)) + ((x1225 * x1227)));
3192  evalcond[2] = ((0.4) + (((-1.0) * x1225 * x1228)) + (((0.4) * cj3)) +
3193  (((-1.0) * x1226 * x1229)) + ((x1226 * x1227)));
3194  evalcond[3] =
3195  (((cj3 * x1231)) + x1231 + x1227 + (((-1.0) * x1225 * x1230)) + (((-1.0) * x1229)));
3196  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3197  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3198  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3199  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3200  {
3201  continue;
3202  }
3203  }
3204 
3205  rotationfunction0(solutions);
3206  }
3207  }
3208  }
3209  }
3210  }
3211  }
3212  }
3213  }
3214  }
3215  }
3216  }
3217  return solutions.GetNumSolutions() > 0;
3218  }
3219  inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
3220  {
3221  for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
3222  {
3223  IkReal x76 = (r02 * sj5);
3224  IkReal x77 = ((1.0) * cj5);
3225  IkReal x78 = ((1.0) * sj6);
3226  IkReal x79 = ((1.0) * cj4);
3227  IkReal x80 = ((1.0) * cj3);
3228  IkReal x81 = ((1.0) * sj4);
3229  IkReal x82 = ((1.0) * cj6);
3230  IkReal x83 = ((((-1.0) * r01 * x78)) + ((cj6 * r00)));
3231  IkReal x84 = (((cj6 * r10)) + (((-1.0) * r11 * x78)));
3232  IkReal x85 = ((((-1.0) * r21 * x78)) + ((cj6 * r20)));
3233  IkReal x86 = ((((-1.0) * r00 * x78)) + (((-1.0) * r01 * x82)));
3234  IkReal x87 = ((((-1.0) * r11 * x82)) + (((-1.0) * r10 * x78)));
3235  IkReal x88 = ((((-1.0) * r20 * x78)) + (((-1.0) * r21 * x82)));
3236  IkReal x89 = (((sj5 * x83)) + ((cj5 * r02)));
3237  IkReal x90 = ((((-1.0) * x77 * x83)) + x76);
3238  IkReal x91 = ((((-1.0) * x77 * x84)) + ((r12 * sj5)));
3239  IkReal x92 = (((cj5 * r12)) + ((sj5 * x84)));
3240  IkReal x93 = (((r22 * sj5)) + (((-1.0) * x77 * x85)));
3241  IkReal x94 = (((cj5 * r22)) + ((sj5 * x85)));
3242  IkReal x95 = (sj4 * x86);
3243  IkReal x96 = ((1.0) * x88);
3244  IkReal x97 = ((((-1.0) * x81 * x87)) + ((cj4 * x91)));
3245  IkReal x98 = ((((-1.0) * x81 * x88)) + ((cj4 * x93)));
3246  new_r00 = ((((-1.0) * x80 * ((((cj4 * (((((-1.0) * cj5 * x83)) + x76)))) + (((-1.0) * x95)))))) + ((sj3 * x89)));
3247  new_r01 = ((((-1.0) * x79 * x86)) + (((-1.0) * x81 * x90)));
3248  new_r02 = (((cj3 * x89)) + ((sj3 * (((((-1.0) * x81 * x86)) + ((cj4 * x90)))))));
3249  new_r10 = (((sj3 * x92)) + (((-1.0) * x80 * x97)));
3250  new_r11 = ((((-1.0) * x79 * x87)) + (((-1.0) * x81 * x91)));
3251  new_r12 = (((sj3 * x97)) + ((cj3 * x92)));
3252  new_r20 = (((sj3 * x94)) + (((-1.0) * x80 * x98)));
3253  new_r21 = ((((-1.0) * x79 * x88)) + (((-1.0) * x81 * x93)));
3254  new_r22 = (((sj3 * x98)) + ((cj3 * x94)));
3255  {
3256  IkReal j1array[2], cj1array[2], sj1array[2];
3257  bool j1valid[2] = { false };
3258  _nj1 = 2;
3259  cj1array[0] = new_r22;
3260  if (cj1array[0] >= -1 - IKFAST_SINCOS_THRESH && cj1array[0] <= 1 + IKFAST_SINCOS_THRESH)
3261  {
3262  j1valid[0] = j1valid[1] = true;
3263  j1array[0] = IKacos(cj1array[0]);
3264  sj1array[0] = IKsin(j1array[0]);
3265  cj1array[1] = cj1array[0];
3266  j1array[1] = -j1array[0];
3267  sj1array[1] = -sj1array[0];
3268  }
3269  else if (isnan(cj1array[0]))
3270  {
3271  // probably any value will work
3272  j1valid[0] = true;
3273  cj1array[0] = 1;
3274  sj1array[0] = 0;
3275  j1array[0] = 0;
3276  }
3277  for (int ij1 = 0; ij1 < 2; ++ij1)
3278  {
3279  if (!j1valid[ij1])
3280  {
3281  continue;
3282  }
3283  _ij1[0] = ij1;
3284  _ij1[1] = -1;
3285  for (int iij1 = ij1 + 1; iij1 < 2; ++iij1)
3286  {
3287  if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
3288  IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
3289  {
3290  j1valid[iij1] = false;
3291  _ij1[1] = iij1;
3292  break;
3293  }
3294  }
3295  j1 = j1array[ij1];
3296  cj1 = cj1array[ij1];
3297  sj1 = sj1array[ij1];
3298 
3299  {
3300  IkReal j0eval[3];
3301  j0eval[0] = sj1;
3302  j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3303  j0eval[2] = IKsign(sj1);
3304  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
3305  IKabs(j0eval[2]) < 0.0000010000000000)
3306  {
3307  {
3308  IkReal j2eval[3];
3309  j2eval[0] = sj1;
3310  j2eval[1] = IKsign(sj1);
3311  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
3312  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
3313  IKabs(j2eval[2]) < 0.0000010000000000)
3314  {
3315  {
3316  IkReal j0eval[2];
3317  j0eval[0] = new_r12;
3318  j0eval[1] = sj1;
3319  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
3320  {
3321  {
3322  IkReal evalcond[5];
3323  bool bgotonextstatement = true;
3324  do
3325  {
3326  evalcond[0] =
3327  ((-3.14159265358979) + (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
3328  evalcond[1] = new_r21;
3329  evalcond[2] = new_r02;
3330  evalcond[3] = new_r12;
3331  evalcond[4] = new_r20;
3332  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3333  IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3334  IKabs(evalcond[4]) < 0.0000050000000000)
3335  {
3336  bgotonextstatement = false;
3337  IkReal j2mul = 1;
3338  j2 = 0;
3339  j0mul = -1.0;
3340  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
3341  IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3342  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r00)) - 1) <=
3343  IKFAST_SINCOS_THRESH)
3344  continue;
3345  j0 = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
3346  {
3347  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3348  vinfos[0].jointtype = 1;
3349  vinfos[0].foffset = j0;
3350  vinfos[0].fmul = j0mul;
3351  vinfos[0].freeind = 0;
3352  vinfos[0].maxsolutions = 0;
3353  vinfos[1].jointtype = 1;
3354  vinfos[1].foffset = j1;
3355  vinfos[1].indices[0] = _ij1[0];
3356  vinfos[1].indices[1] = _ij1[1];
3357  vinfos[1].maxsolutions = _nj1;
3358  vinfos[2].jointtype = 1;
3359  vinfos[2].foffset = j2;
3360  vinfos[2].fmul = j2mul;
3361  vinfos[2].freeind = 0;
3362  vinfos[2].maxsolutions = 0;
3363  vinfos[3].jointtype = 1;
3364  vinfos[3].foffset = j3;
3365  vinfos[3].indices[0] = _ij3[0];
3366  vinfos[3].indices[1] = _ij3[1];
3367  vinfos[3].maxsolutions = _nj3;
3368  vinfos[4].jointtype = 1;
3369  vinfos[4].foffset = j4;
3370  vinfos[4].indices[0] = _ij4[0];
3371  vinfos[4].indices[1] = _ij4[1];
3372  vinfos[4].maxsolutions = _nj4;
3373  vinfos[5].jointtype = 1;
3374  vinfos[5].foffset = j5;
3375  vinfos[5].indices[0] = _ij5[0];
3376  vinfos[5].indices[1] = _ij5[1];
3377  vinfos[5].maxsolutions = _nj5;
3378  vinfos[6].jointtype = 1;
3379  vinfos[6].foffset = j6;
3380  vinfos[6].indices[0] = _ij6[0];
3381  vinfos[6].indices[1] = _ij6[1];
3382  vinfos[6].maxsolutions = _nj6;
3383  std::vector<int> vfree(1);
3384  vfree[0] = 2;
3385  solutions.AddSolution(vinfos, vfree);
3386  }
3387  }
3388  } while (0);
3389  if (bgotonextstatement)
3390  {
3391  bool bgotonextstatement = true;
3392  do
3393  {
3394  evalcond[0] = ((-3.14159265358979) +
3395  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
3396  6.28318530717959)));
3397  evalcond[1] = new_r21;
3398  evalcond[2] = new_r02;
3399  evalcond[3] = new_r12;
3400  evalcond[4] = new_r20;
3401  if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3402  IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3403  IKabs(evalcond[4]) < 0.0000050000000000)
3404  {
3405  bgotonextstatement = false;
3406  IkReal j2mul = 1;
3407  j2 = 0;
3408  j0mul = 1.0;
3409  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
3410  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
3411  IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) - 1) <= IKFAST_SINCOS_THRESH)
3412  continue;
3413  j0 = IKatan2(new_r10, ((-1.0) * new_r11));
3414  {
3415  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3416  vinfos[0].jointtype = 1;
3417  vinfos[0].foffset = j0;
3418  vinfos[0].fmul = j0mul;
3419  vinfos[0].freeind = 0;
3420  vinfos[0].maxsolutions = 0;
3421  vinfos[1].jointtype = 1;
3422  vinfos[1].foffset = j1;
3423  vinfos[1].indices[0] = _ij1[0];
3424  vinfos[1].indices[1] = _ij1[1];
3425  vinfos[1].maxsolutions = _nj1;
3426  vinfos[2].jointtype = 1;
3427  vinfos[2].foffset = j2;
3428  vinfos[2].fmul = j2mul;
3429  vinfos[2].freeind = 0;
3430  vinfos[2].maxsolutions = 0;
3431  vinfos[3].jointtype = 1;
3432  vinfos[3].foffset = j3;
3433  vinfos[3].indices[0] = _ij3[0];
3434  vinfos[3].indices[1] = _ij3[1];
3435  vinfos[3].maxsolutions = _nj3;
3436  vinfos[4].jointtype = 1;
3437  vinfos[4].foffset = j4;
3438  vinfos[4].indices[0] = _ij4[0];
3439  vinfos[4].indices[1] = _ij4[1];
3440  vinfos[4].maxsolutions = _nj4;
3441  vinfos[5].jointtype = 1;
3442  vinfos[5].foffset = j5;
3443  vinfos[5].indices[0] = _ij5[0];
3444  vinfos[5].indices[1] = _ij5[1];
3445  vinfos[5].maxsolutions = _nj5;
3446  vinfos[6].jointtype = 1;
3447  vinfos[6].foffset = j6;
3448  vinfos[6].indices[0] = _ij6[0];
3449  vinfos[6].indices[1] = _ij6[1];
3450  vinfos[6].maxsolutions = _nj6;
3451  std::vector<int> vfree(1);
3452  vfree[0] = 2;
3453  solutions.AddSolution(vinfos, vfree);
3454  }
3455  }
3456  } while (0);
3457  if (bgotonextstatement)
3458  {
3459  bool bgotonextstatement = true;
3460  do
3461  {
3462  evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3463  if (IKabs(evalcond[0]) < 0.0000050000000000)
3464  {
3465  bgotonextstatement = false;
3466  {
3467  IkReal j0eval[1];
3468  new_r02 = 0;
3469  new_r12 = 0;
3470  new_r20 = 0;
3471  new_r21 = 0;
3472  IkReal x99 = new_r22 * new_r22;
3473  IkReal x100 = ((16.0) * new_r10);
3474  IkReal x101 = ((16.0) * new_r01);
3475  IkReal x102 = ((16.0) * new_r22);
3476  IkReal x103 = ((8.0) * new_r11);
3477  IkReal x104 = ((8.0) * new_r00);
3478  IkReal x105 = (x100 * x99);
3479  IkReal x106 = (x101 * x99);
3480  j0eval[0] =
3481  ((IKabs(
3482  ((((16.0) * new_r00)) + (((-32.0) * new_r00 * x99)) + ((new_r11 * x102))))) +
3483  (IKabs(((((-1.0) * x105)) + x100))) + (IKabs(((((-1.0) * x100)) + x105))) +
3484  (IKabs(((((32.0) * new_r11)) + (((-1.0) * new_r00 * x102)) +
3485  (((-16.0) * new_r11 * x99))))) +
3486  (IKabs(((((-1.0) * x101)) + x106))) +
3487  (IKabs((((x103 * x99)) + (((-1.0) * new_r22 * x104))))) +
3488  (IKabs((((new_r22 * x103)) + (((-1.0) * x104))))) +
3489  (IKabs(((((-1.0) * x106)) + x101))));
3490  if (IKabs(j0eval[0]) < 0.0000000100000000)
3491  {
3492  continue; // no branches [j0, j2]
3493  }
3494  else
3495  {
3496  IkReal op[4 + 1], zeror[4];
3497  int numroots;
3498  IkReal j0evalpoly[1];
3499  IkReal x107 = new_r22 * new_r22;
3500  IkReal x108 = ((16.0) * new_r10);
3501  IkReal x109 = (new_r11 * new_r22);
3502  IkReal x110 = (x107 * x108);
3503  IkReal x111 = ((((8.0) * x109)) + (((-8.0) * new_r00)));
3504  op[0] = x111;
3505  op[1] = ((((-1.0) * x110)) + x108);
3506  op[2] = ((((16.0) * x109)) + (((-32.0) * new_r00 * x107)) + (((16.0) * new_r00)));
3507  op[3] = ((((-1.0) * x108)) + x110);
3508  op[4] = x111;
3509  polyroots4(op, zeror, numroots);
3510  IkReal j0array[4], cj0array[4], sj0array[4], tempj0array[1];
3511  int numsolutions = 0;
3512  for (int ij0 = 0; ij0 < numroots; ++ij0)
3513  {
3514  IkReal htj0 = zeror[ij0];
3515  tempj0array[0] = ((2.0) * (atan(htj0)));
3516  for (int kj0 = 0; kj0 < 1; ++kj0)
3517  {
3518  j0array[numsolutions] = tempj0array[kj0];
3519  if (j0array[numsolutions] > IKPI)
3520  {
3521  j0array[numsolutions] -= IK2PI;
3522  }
3523  else if (j0array[numsolutions] < -IKPI)
3524  {
3525  j0array[numsolutions] += IK2PI;
3526  }
3527  sj0array[numsolutions] = IKsin(j0array[numsolutions]);
3528  cj0array[numsolutions] = IKcos(j0array[numsolutions]);
3529  numsolutions++;
3530  }
3531  }
3532  bool j0valid[4] = { true, true, true, true };
3533  _nj0 = 4;
3534  for (int ij0 = 0; ij0 < numsolutions; ++ij0)
3535  {
3536  if (!j0valid[ij0])
3537  {
3538  continue;
3539  }
3540  j0 = j0array[ij0];
3541  cj0 = cj0array[ij0];
3542  sj0 = sj0array[ij0];
3543  htj0 = IKtan(j0 / 2);
3544 
3545  IkReal x112 = ((16.0) * new_r01);
3546  IkReal x113 = new_r22 * new_r22;
3547  IkReal x114 = (new_r00 * new_r22);
3548  IkReal x115 = ((8.0) * x114);
3549  IkReal x116 = (new_r11 * x113);
3550  IkReal x117 = (x112 * x113);
3551  IkReal x118 = ((8.0) * x116);
3552  j0evalpoly[0] =
3553  (((htj0 * (((((-1.0) * x117)) + x112)))) +
3554  (((htj0 * htj0 * htj0) * (((((-1.0) * x112)) + x117)))) + (((-1.0) * x115)) +
3555  x118 +
3556  (((htj0 * htj0) *
3557  (((((32.0) * new_r11)) + (((-16.0) * x114)) + (((-16.0) * x116)))))) +
3558  (((htj0 * htj0 * htj0 * htj0) * (((((-1.0) * x115)) + x118)))));
3559  if (IKabs(j0evalpoly[0]) > 0.0000001000000000)
3560  {
3561  continue;
3562  }
3563  _ij0[0] = ij0;
3564  _ij0[1] = -1;
3565  for (int iij0 = ij0 + 1; iij0 < numsolutions; ++iij0)
3566  {
3567  if (j0valid[iij0] &&
3568  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
3569  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
3570  {
3571  j0valid[iij0] = false;
3572  _ij0[1] = iij0;
3573  break;
3574  }
3575  }
3576  {
3577  IkReal j2eval[3];
3578  new_r02 = 0;
3579  new_r12 = 0;
3580  new_r20 = 0;
3581  new_r21 = 0;
3582  IkReal x119 = cj0 * cj0;
3583  IkReal x120 = new_r22 * new_r22;
3584  IkReal x121 = ((1.0) * cj0);
3585  IkReal x122 = (new_r22 * sj0);
3586  IkReal x123 = (x120 + x119 + (((-1.0) * x119 * x120)));
3587  j2eval[0] = x123;
3588  j2eval[1] =
3589  ((IKabs(((((-1.0) * new_r10 * x122)) + (((-1.0) * new_r11 * x121))))) +
3590  (IKabs(((((-1.0) * new_r10 * x121)) + ((new_r11 * x122))))));
3591  j2eval[2] = IKsign(x123);
3592  if (IKabs(j2eval[0]) < 0.0000010000000000 ||
3593  IKabs(j2eval[1]) < 0.0000010000000000 ||
3594  IKabs(j2eval[2]) < 0.0000010000000000)
3595  {
3596  {
3597  IkReal j2eval[1];
3598  new_r02 = 0;
3599  new_r12 = 0;
3600  new_r20 = 0;
3601  new_r21 = 0;
3602  j2eval[0] = new_r22;
3603  if (IKabs(j2eval[0]) < 0.0000010000000000)
3604  {
3605  {
3606  IkReal j2eval[1];
3607  new_r02 = 0;
3608  new_r12 = 0;
3609  new_r20 = 0;
3610  new_r21 = 0;
3611  j2eval[0] = cj0;
3612  if (IKabs(j2eval[0]) < 0.0000010000000000)
3613  {
3614  {
3615  IkReal evalcond[1];
3616  bool bgotonextstatement = true;
3617  do
3618  {
3619  evalcond[0] = ((-3.14159265358979) +
3620  (IKfmod(((3.14159265358979) +
3621  (IKabs(((-1.5707963267949) + j0)))),
3622  6.28318530717959)));
3623  if (IKabs(evalcond[0]) < 0.0000050000000000)
3624  {
3625  bgotonextstatement = false;
3626  {
3627  IkReal j2array[1], cj2array[1], sj2array[1];
3628  bool j2valid[1] = { false };
3629  _nj2 = 1;
3630  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
3631  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
3632  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
3633  IKFAST_SINCOS_THRESH)
3634  continue;
3635  j2array[0] = IKatan2(new_r00, new_r01);
3636  sj2array[0] = IKsin(j2array[0]);
3637  cj2array[0] = IKcos(j2array[0]);
3638  if (j2array[0] > IKPI)
3639  {
3640  j2array[0] -= IK2PI;
3641  }
3642  else if (j2array[0] < -IKPI)
3643  {
3644  j2array[0] += IK2PI;
3645  }
3646  j2valid[0] = true;
3647  for (int ij2 = 0; ij2 < 1; ++ij2)
3648  {
3649  if (!j2valid[ij2])
3650  {
3651  continue;
3652  }
3653  _ij2[0] = ij2;
3654  _ij2[1] = -1;
3655  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3656  {
3657  if (j2valid[iij2] &&
3658  IKabs(cj2array[ij2] - cj2array[iij2]) <
3659  IKFAST_SOLUTION_THRESH &&
3660  IKabs(sj2array[ij2] - sj2array[iij2]) <
3661  IKFAST_SOLUTION_THRESH)
3662  {
3663  j2valid[iij2] = false;
3664  _ij2[1] = iij2;
3665  break;
3666  }
3667  }
3668  j2 = j2array[ij2];
3669  cj2 = cj2array[ij2];
3670  sj2 = sj2array[ij2];
3671  {
3672  IkReal evalcond[4];
3673  IkReal x124 = IKsin(j2);
3674  IkReal x125 = IKcos(j2);
3675  evalcond[0] = x125;
3676  evalcond[1] = ((-1.0) * x124);
3677  evalcond[2] = (x124 + (((-1.0) * new_r00)));
3678  evalcond[3] = (x125 + (((-1.0) * new_r01)));
3679  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3680  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3681  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3682  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3683  {
3684  continue;
3685  }
3686  }
3687 
3688  {
3689  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3690  vinfos[0].jointtype = 1;
3691  vinfos[0].foffset = j0;
3692  vinfos[0].indices[0] = _ij0[0];
3693  vinfos[0].indices[1] = _ij0[1];
3694  vinfos[0].maxsolutions = _nj0;
3695  vinfos[1].jointtype = 1;
3696  vinfos[1].foffset = j1;
3697  vinfos[1].indices[0] = _ij1[0];
3698  vinfos[1].indices[1] = _ij1[1];
3699  vinfos[1].maxsolutions = _nj1;
3700  vinfos[2].jointtype = 1;
3701  vinfos[2].foffset = j2;
3702  vinfos[2].indices[0] = _ij2[0];
3703  vinfos[2].indices[1] = _ij2[1];
3704  vinfos[2].maxsolutions = _nj2;
3705  vinfos[3].jointtype = 1;
3706  vinfos[3].foffset = j3;
3707  vinfos[3].indices[0] = _ij3[0];
3708  vinfos[3].indices[1] = _ij3[1];
3709  vinfos[3].maxsolutions = _nj3;
3710  vinfos[4].jointtype = 1;
3711  vinfos[4].foffset = j4;
3712  vinfos[4].indices[0] = _ij4[0];
3713  vinfos[4].indices[1] = _ij4[1];
3714  vinfos[4].maxsolutions = _nj4;
3715  vinfos[5].jointtype = 1;
3716  vinfos[5].foffset = j5;
3717  vinfos[5].indices[0] = _ij5[0];
3718  vinfos[5].indices[1] = _ij5[1];
3719  vinfos[5].maxsolutions = _nj5;
3720  vinfos[6].jointtype = 1;
3721  vinfos[6].foffset = j6;
3722  vinfos[6].indices[0] = _ij6[0];
3723  vinfos[6].indices[1] = _ij6[1];
3724  vinfos[6].maxsolutions = _nj6;
3725  std::vector<int> vfree(0);
3726  solutions.AddSolution(vinfos, vfree);
3727  }
3728  }
3729  }
3730  }
3731  } while (0);
3732  if (bgotonextstatement)
3733  {
3734  bool bgotonextstatement = true;
3735  do
3736  {
3737  evalcond[0] = ((-3.14159265358979) +
3738  (IKfmod(((3.14159265358979) +
3739  (IKabs(((1.5707963267949) + j0)))),
3740  6.28318530717959)));
3741  if (IKabs(evalcond[0]) < 0.0000050000000000)
3742  {
3743  bgotonextstatement = false;
3744  {
3745  IkReal j2array[1], cj2array[1], sj2array[1];
3746  bool j2valid[1] = { false };
3747  _nj2 = 1;
3748  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3749  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
3750  IKabs(IKsqr(((-1.0) * new_r00)) +
3751  IKsqr(((-1.0) * new_r01)) - 1) <=
3752  IKFAST_SINCOS_THRESH)
3753  continue;
3754  j2array[0] =
3755  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
3756  sj2array[0] = IKsin(j2array[0]);
3757  cj2array[0] = IKcos(j2array[0]);
3758  if (j2array[0] > IKPI)
3759  {
3760  j2array[0] -= IK2PI;
3761  }
3762  else if (j2array[0] < -IKPI)
3763  {
3764  j2array[0] += IK2PI;
3765  }
3766  j2valid[0] = true;
3767  for (int ij2 = 0; ij2 < 1; ++ij2)
3768  {
3769  if (!j2valid[ij2])
3770  {
3771  continue;
3772  }
3773  _ij2[0] = ij2;
3774  _ij2[1] = -1;
3775  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3776  {
3777  if (j2valid[iij2] &&
3778  IKabs(cj2array[ij2] - cj2array[iij2]) <
3779  IKFAST_SOLUTION_THRESH &&
3780  IKabs(sj2array[ij2] - sj2array[iij2]) <
3781  IKFAST_SOLUTION_THRESH)
3782  {
3783  j2valid[iij2] = false;
3784  _ij2[1] = iij2;
3785  break;
3786  }
3787  }
3788  j2 = j2array[ij2];
3789  cj2 = cj2array[ij2];
3790  sj2 = sj2array[ij2];
3791  {
3792  IkReal evalcond[4];
3793  IkReal x126 = IKcos(j2);
3794  IkReal x127 = IKsin(j2);
3795  evalcond[0] = x126;
3796  evalcond[1] = (x127 + new_r00);
3797  evalcond[2] = (x126 + new_r01);
3798  evalcond[3] = ((-1.0) * x127);
3799  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3800  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3801  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3802  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3803  {
3804  continue;
3805  }
3806  }
3807 
3808  {
3809  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3810  vinfos[0].jointtype = 1;
3811  vinfos[0].foffset = j0;
3812  vinfos[0].indices[0] = _ij0[0];
3813  vinfos[0].indices[1] = _ij0[1];
3814  vinfos[0].maxsolutions = _nj0;
3815  vinfos[1].jointtype = 1;
3816  vinfos[1].foffset = j1;
3817  vinfos[1].indices[0] = _ij1[0];
3818  vinfos[1].indices[1] = _ij1[1];
3819  vinfos[1].maxsolutions = _nj1;
3820  vinfos[2].jointtype = 1;
3821  vinfos[2].foffset = j2;
3822  vinfos[2].indices[0] = _ij2[0];
3823  vinfos[2].indices[1] = _ij2[1];
3824  vinfos[2].maxsolutions = _nj2;
3825  vinfos[3].jointtype = 1;
3826  vinfos[3].foffset = j3;
3827  vinfos[3].indices[0] = _ij3[0];
3828  vinfos[3].indices[1] = _ij3[1];
3829  vinfos[3].maxsolutions = _nj3;
3830  vinfos[4].jointtype = 1;
3831  vinfos[4].foffset = j4;
3832  vinfos[4].indices[0] = _ij4[0];
3833  vinfos[4].indices[1] = _ij4[1];
3834  vinfos[4].maxsolutions = _nj4;
3835  vinfos[5].jointtype = 1;
3836  vinfos[5].foffset = j5;
3837  vinfos[5].indices[0] = _ij5[0];
3838  vinfos[5].indices[1] = _ij5[1];
3839  vinfos[5].maxsolutions = _nj5;
3840  vinfos[6].jointtype = 1;
3841  vinfos[6].foffset = j6;
3842  vinfos[6].indices[0] = _ij6[0];
3843  vinfos[6].indices[1] = _ij6[1];
3844  vinfos[6].maxsolutions = _nj6;
3845  std::vector<int> vfree(0);
3846  solutions.AddSolution(vinfos, vfree);
3847  }
3848  }
3849  }
3850  }
3851  } while (0);
3852  if (bgotonextstatement)
3853  {
3854  bool bgotonextstatement = true;
3855  do
3856  {
3857  IkReal x128 = new_r22 * new_r22;
3858  CheckValue<IkReal> x129 =
3859  IKPowWithIntegerCheck(((1.0) + (((-1.0) * x128))), -1);
3860  if (!x129.valid)
3861  {
3862  continue;
3863  }
3864  if ((((-1.0) * x128 * (x129.value))) < -0.00001)
3865  continue;
3866  IkReal gconst12 = IKsqrt(((-1.0) * x128 * (x129.value)));
3867  evalcond[0] =
3868  ((-3.14159265358979) +
3869  (IKfmod(((3.14159265358979) +
3870  (IKabs((cj0 + (((-1.0) * gconst12))))) +
3871  (IKabs(((-1.0) + (IKsign(sj0)))))),
3872  6.28318530717959)));
3873  if (IKabs(evalcond[0]) < 0.0000050000000000)
3874  {
3875  bgotonextstatement = false;
3876  {
3877  IkReal j2eval[1];
3878  IkReal x130 = new_r22 * new_r22;
3879  new_r02 = 0;
3880  new_r12 = 0;
3881  new_r20 = 0;
3882  new_r21 = 0;
3883  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3884  -0.00001)
3885  continue;
3886  sj0 =
3887  IKsqrt(((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3888  cj0 = gconst12;
3889  if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
3890  (gconst12) > 1 + IKFAST_SINCOS_THRESH)
3891  continue;
3892  j0 = IKacos(gconst12);
3893  CheckValue<IkReal> x131 = IKPowWithIntegerCheck(
3894  ((1.0) + (((-1.0) * x130))), -1);
3895  if (!x131.valid)
3896  {
3897  continue;
3898  }
3899  if ((((-1.0) * x130 * (x131.value))) < -0.00001)
3900  continue;
3901  IkReal gconst12 = IKsqrt(((-1.0) * x130 * (x131.value)));
3902  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
3903  if (IKabs(j2eval[0]) < 0.0000010000000000)
3904  {
3905  {
3906  IkReal j2array[1], cj2array[1], sj2array[1];
3907  bool j2valid[1] = { false };
3908  _nj2 = 1;
3909  CheckValue<IkReal> x132 =
3910  IKPowWithIntegerCheck(gconst12, -1);
3911  if (!x132.valid)
3912  {
3913  continue;
3914  }
3915  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3916  -0.00001)
3917  continue;
3918  if (IKabs(((-1.0) * new_r10 * (x132.value))) <
3919  IKFAST_ATAN2_MAGTHRESH &&
3920  IKabs((((new_r01 *
3921  (IKsqrt(((1.0) +
3922  (((-1.0) *
3923  (gconst12 * gconst12)))))))) +
3924  (((-1.0) * gconst12 * new_r11)))) <
3925  IKFAST_ATAN2_MAGTHRESH &&
3926  IKabs(IKsqr(((-1.0) * new_r10 * (x132.value))) +
3927  IKsqr((
3928  ((new_r01 *
3929  (IKsqrt(((1.0) + (((-1.0) *
3930  (gconst12 *
3931  gconst12)))))))) +
3932  (((-1.0) * gconst12 * new_r11)))) -
3933  1) <= IKFAST_SINCOS_THRESH)
3934  continue;
3935  j2array[0] = IKatan2(
3936  ((-1.0) * new_r10 * (x132.value)),
3937  (((new_r01 *
3938  (IKsqrt(((1.0) + (((-1.0) * (gconst12 *
3939  gconst12)))))))) +
3940  (((-1.0) * gconst12 * new_r11))));
3941  sj2array[0] = IKsin(j2array[0]);
3942  cj2array[0] = IKcos(j2array[0]);
3943  if (j2array[0] > IKPI)
3944  {
3945  j2array[0] -= IK2PI;
3946  }
3947  else if (j2array[0] < -IKPI)
3948  {
3949  j2array[0] += IK2PI;
3950  }
3951  j2valid[0] = true;
3952  for (int ij2 = 0; ij2 < 1; ++ij2)
3953  {
3954  if (!j2valid[ij2])
3955  {
3956  continue;
3957  }
3958  _ij2[0] = ij2;
3959  _ij2[1] = -1;
3960  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3961  {
3962  if (j2valid[iij2] &&
3963  IKabs(cj2array[ij2] - cj2array[iij2]) <
3964  IKFAST_SOLUTION_THRESH &&
3965  IKabs(sj2array[ij2] - sj2array[iij2]) <
3966  IKFAST_SOLUTION_THRESH)
3967  {
3968  j2valid[iij2] = false;
3969  _ij2[1] = iij2;
3970  break;
3971  }
3972  }
3973  j2 = j2array[ij2];
3974  cj2 = cj2array[ij2];
3975  sj2 = sj2array[ij2];
3976  {
3977  IkReal evalcond[8];
3978  IkReal x133 = IKsin(j2);
3979  IkReal x134 = IKcos(j2);
3980  if ((((1.0) +
3981  (((-1.0) * (gconst12 * gconst12))))) <
3982  -0.00001)
3983  continue;
3984  IkReal x135 = IKsqrt(
3985  ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3986  IkReal x136 = ((1.0) * x135);
3987  evalcond[0] = x134;
3988  evalcond[1] = ((-1.0) * x133);
3989  evalcond[2] = (((gconst12 * x133)) + new_r10);
3990  evalcond[3] = (((gconst12 * x134)) + new_r11);
3991  evalcond[4] =
3992  ((((-1.0) * x133 * x136)) + new_r00);
3993  evalcond[5] =
3994  ((((-1.0) * x134 * x136)) + new_r01);
3995  evalcond[6] = ((((-1.0) * new_r00 * x136)) +
3996  x133 + ((gconst12 * new_r10)));
3997  evalcond[7] = ((((-1.0) * new_r01 * x136)) +
3998  x134 + ((gconst12 * new_r11)));
3999  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
4000  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4001  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4002  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4003  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4004  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4005  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4006  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4007  {
4008  continue;
4009  }
4010  }
4011 
4012  {
4013  std::vector<IkSingleDOFSolutionBase<IkReal> >
4014  vinfos(7);
4015  vinfos[0].jointtype = 1;
4016  vinfos[0].foffset = j0;
4017  vinfos[0].indices[0] = _ij0[0];
4018  vinfos[0].indices[1] = _ij0[1];
4019  vinfos[0].maxsolutions = _nj0;
4020  vinfos[1].jointtype = 1;
4021  vinfos[1].foffset = j1;
4022  vinfos[1].indices[0] = _ij1[0];
4023  vinfos[1].indices[1] = _ij1[1];
4024  vinfos[1].maxsolutions = _nj1;
4025  vinfos[2].jointtype = 1;
4026  vinfos[2].foffset = j2;
4027  vinfos[2].indices[0] = _ij2[0];
4028  vinfos[2].indices[1] = _ij2[1];
4029  vinfos[2].maxsolutions = _nj2;
4030  vinfos[3].jointtype = 1;
4031  vinfos[3].foffset = j3;
4032  vinfos[3].indices[0] = _ij3[0];
4033  vinfos[3].indices[1] = _ij3[1];
4034  vinfos[3].maxsolutions = _nj3;
4035  vinfos[4].jointtype = 1;
4036  vinfos[4].foffset = j4;
4037  vinfos[4].indices[0] = _ij4[0];
4038  vinfos[4].indices[1] = _ij4[1];
4039  vinfos[4].maxsolutions = _nj4;
4040  vinfos[5].jointtype = 1;
4041  vinfos[5].foffset = j5;
4042  vinfos[5].indices[0] = _ij5[0];
4043  vinfos[5].indices[1] = _ij5[1];
4044  vinfos[5].maxsolutions = _nj5;
4045  vinfos[6].jointtype = 1;
4046  vinfos[6].foffset = j6;
4047  vinfos[6].indices[0] = _ij6[0];
4048  vinfos[6].indices[1] = _ij6[1];
4049  vinfos[6].maxsolutions = _nj6;
4050  std::vector<int> vfree(0);
4051  solutions.AddSolution(vinfos, vfree);
4052  }
4053  }
4054  }
4055  }
4056  else
4057  {
4058  {
4059  IkReal j2array[1], cj2array[1], sj2array[1];
4060  bool j2valid[1] = { false };
4061  _nj2 = 1;
4062  CheckValue<IkReal> x137 =
4063  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4064  IkReal(((-1.0) * new_r11)),
4065  IKFAST_ATAN2_MAGTHRESH);
4066  if (!x137.valid)
4067  {
4068  continue;
4069  }
4070  CheckValue<IkReal> x138 =
4071  IKPowWithIntegerCheck(IKsign(gconst12), -1);
4072  if (!x138.valid)
4073  {
4074  continue;
4075  }
4076  j2array[0] = ((-1.5707963267949) + (x137.value) +
4077  (((1.5707963267949) * (x138.value))));
4078  sj2array[0] = IKsin(j2array[0]);
4079  cj2array[0] = IKcos(j2array[0]);
4080  if (j2array[0] > IKPI)
4081  {
4082  j2array[0] -= IK2PI;
4083  }
4084  else if (j2array[0] < -IKPI)
4085  {
4086  j2array[0] += IK2PI;
4087  }
4088  j2valid[0] = true;
4089  for (int ij2 = 0; ij2 < 1; ++ij2)
4090  {
4091  if (!j2valid[ij2])
4092  {
4093  continue;
4094  }
4095  _ij2[0] = ij2;
4096  _ij2[1] = -1;
4097  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4098  {
4099  if (j2valid[iij2] &&
4100  IKabs(cj2array[ij2] - cj2array[iij2]) <
4101  IKFAST_SOLUTION_THRESH &&
4102  IKabs(sj2array[ij2] - sj2array[iij2]) <
4103  IKFAST_SOLUTION_THRESH)
4104  {
4105  j2valid[iij2] = false;
4106  _ij2[1] = iij2;
4107  break;
4108  }
4109  }
4110  j2 = j2array[ij2];
4111  cj2 = cj2array[ij2];
4112  sj2 = sj2array[ij2];
4113  {
4114  IkReal evalcond[8];
4115  IkReal x139 = IKsin(j2);
4116  IkReal x140 = IKcos(j2);
4117  if ((((1.0) +
4118  (((-1.0) * (gconst12 * gconst12))))) <
4119  -0.00001)
4120  continue;
4121  IkReal x141 = IKsqrt(
4122  ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
4123  IkReal x142 = ((1.0) * x141);
4124  evalcond[0] = x140;
4125  evalcond[1] = ((-1.0) * x139);
4126  evalcond[2] = (((gconst12 * x139)) + new_r10);
4127  evalcond[3] = (((gconst12 * x140)) + new_r11);
4128  evalcond[4] =
4129  ((((-1.0) * x139 * x142)) + new_r00);
4130  evalcond[5] =
4131  ((((-1.0) * x140 * x142)) + new_r01);
4132  evalcond[6] =
4133  (x139 + (((-1.0) * new_r00 * x142)) +
4134  ((gconst12 * new_r10)));
4135  evalcond[7] = ((((-1.0) * new_r01 * x142)) +
4136  x140 + ((gconst12 * new_r11)));
4137  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
4138  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4139  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4140  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4141  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4142  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4143  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4144  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4145  {
4146  continue;
4147  }
4148  }
4149 
4150  {
4151  std::vector<IkSingleDOFSolutionBase<IkReal> >
4152  vinfos(7);
4153  vinfos[0].jointtype = 1;
4154  vinfos[0].foffset = j0;
4155  vinfos[0].indices[0] = _ij0[0];
4156  vinfos[0].indices[1] = _ij0[1];
4157  vinfos[0].maxsolutions = _nj0;
4158  vinfos[1].jointtype = 1;
4159  vinfos[1].foffset = j1;
4160  vinfos[1].indices[0] = _ij1[0];
4161  vinfos[1].indices[1] = _ij1[1];
4162  vinfos[1].maxsolutions = _nj1;
4163  vinfos[2].jointtype = 1;
4164  vinfos[2].foffset = j2;
4165  vinfos[2].indices[0] = _ij2[0];
4166  vinfos[2].indices[1] = _ij2[1];
4167  vinfos[2].maxsolutions = _nj2;
4168  vinfos[3].jointtype = 1;
4169  vinfos[3].foffset = j3;
4170  vinfos[3].indices[0] = _ij3[0];
4171  vinfos[3].indices[1] = _ij3[1];
4172  vinfos[3].maxsolutions = _nj3;
4173  vinfos[4].jointtype = 1;
4174  vinfos[4].foffset = j4;
4175  vinfos[4].indices[0] = _ij4[0];
4176  vinfos[4].indices[1] = _ij4[1];
4177  vinfos[4].maxsolutions = _nj4;
4178  vinfos[5].jointtype = 1;
4179  vinfos[5].foffset = j5;
4180  vinfos[5].indices[0] = _ij5[0];
4181  vinfos[5].indices[1] = _ij5[1];
4182  vinfos[5].maxsolutions = _nj5;
4183  vinfos[6].jointtype = 1;
4184  vinfos[6].foffset = j6;
4185  vinfos[6].indices[0] = _ij6[0];
4186  vinfos[6].indices[1] = _ij6[1];
4187  vinfos[6].maxsolutions = _nj6;
4188  std::vector<int> vfree(0);
4189  solutions.AddSolution(vinfos, vfree);
4190  }
4191  }
4192  }
4193  }
4194  }
4195  }
4196  } while (0);
4197  if (bgotonextstatement)
4198  {
4199  bool bgotonextstatement = true;
4200  do
4201  {
4202  IkReal x143 = new_r22 * new_r22;
4203  CheckValue<IkReal> x144 =
4204  IKPowWithIntegerCheck(((1.0) + (((-1.0) * x143))), -1);
4205  if (!x144.valid)
4206  {
4207  continue;
4208  }
4209  if ((((-1.0) * x143 * (x144.value))) < -0.00001)
4210  continue;
4211  IkReal gconst12 = IKsqrt(((-1.0) * x143 * (x144.value)));
4212  evalcond[0] =
4213  ((-3.14159265358979) +
4214  (IKfmod(((3.14159265358979) +
4215  (IKabs((cj0 + (((-1.0) * gconst12))))) +
4216  (IKabs(((1.0) + (IKsign(sj0)))))),
4217  6.28318530717959)));
4218  if (IKabs(evalcond[0]) < 0.0000050000000000)
4219  {
4220  bgotonextstatement = false;
4221  {
4222  IkReal j2eval[1];
4223  IkReal x145 = new_r22 * new_r22;
4224  new_r02 = 0;
4225  new_r12 = 0;
4226  new_r20 = 0;
4227  new_r21 = 0;
4228  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4229  -0.00001)
4230  continue;
4231  sj0 = ((-1.0) *
4232  (IKsqrt(((1.0) +
4233  (((-1.0) * (gconst12 * gconst12)))))));
4234  cj0 = gconst12;
4235  if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
4236  (gconst12) > 1 + IKFAST_SINCOS_THRESH)
4237  continue;
4238  j0 = ((-1.0) * (IKacos(gconst12)));
4239  CheckValue<IkReal> x146 = IKPowWithIntegerCheck(
4240  ((1.0) + (((-1.0) * x145))), -1);
4241  if (!x146.valid)
4242  {
4243  continue;
4244  }
4245  if ((((-1.0) * x145 * (x146.value))) < -0.00001)
4246  continue;
4247  IkReal gconst12 =
4248  IKsqrt(((-1.0) * x145 * (x146.value)));
4249  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4250  if (IKabs(j2eval[0]) < 0.0000010000000000)
4251  {
4252  {
4253  IkReal j2array[1], cj2array[1], sj2array[1];
4254  bool j2valid[1] = { false };
4255  _nj2 = 1;
4256  CheckValue<IkReal> x147 =
4257  IKPowWithIntegerCheck(gconst12, -1);
4258  if (!x147.valid)
4259  {
4260  continue;
4261  }
4262  if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4263  -0.00001)
4264  continue;
4265  if (IKabs(((-1.0) * new_r10 * (x147.value))) <
4266  IKFAST_ATAN2_MAGTHRESH &&
4267  IKabs(((((-1.0) * new_r01 *
4268  (IKsqrt(((1.0) +
4269  (((-1.0) * (gconst12 *
4270  gconst12)))))))) +
4271  (((-1.0) * gconst12 * new_r11)))) <
4272  IKFAST_ATAN2_MAGTHRESH &&
4273  IKabs(IKsqr(((-1.0) * new_r10 * (x147.value))) +
4274  IKsqr(((((-1.0) * new_r01 *
4275  (IKsqrt(((1.0) +
4276  (((-1.0) *
4277  (gconst12 *
4278  gconst12)))))))) +
4279  (((-1.0) * gconst12 * new_r11)))) -
4280  1) <= IKFAST_SINCOS_THRESH)
4281  continue;
4282  j2array[0] = IKatan2(
4283  ((-1.0) * new_r10 * (x147.value)),
4284  ((((-1.0) * new_r01 *
4285  (IKsqrt((
4286  (1.0) +
4287  (((-1.0) * (gconst12 * gconst12)))))))) +
4288  (((-1.0) * gconst12 * new_r11))));
4289  sj2array[0] = IKsin(j2array[0]);
4290  cj2array[0] = IKcos(j2array[0]);
4291  if (j2array[0] > IKPI)
4292  {
4293  j2array[0] -= IK2PI;
4294  }
4295  else if (j2array[0] < -IKPI)
4296  {
4297  j2array[0] += IK2PI;
4298  }
4299  j2valid[0] = true;
4300  for (int ij2 = 0; ij2 < 1; ++ij2)
4301  {
4302  if (!j2valid[ij2])
4303  {
4304  continue;
4305  }
4306  _ij2[0] = ij2;
4307  _ij2[1] = -1;
4308  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4309  {
4310  if (j2valid[iij2] &&
4311  IKabs(cj2array[ij2] - cj2array[iij2]) <
4312  IKFAST_SOLUTION_THRESH &&
4313  IKabs(sj2array[ij2] - sj2array[iij2]) <
4314  IKFAST_SOLUTION_THRESH)
4315  {
4316  j2valid[iij2] = false;
4317  _ij2[1] = iij2;
4318  break;
4319  }
4320  }
4321  j2 = j2array[ij2];
4322  cj2 = cj2array[ij2];
4323  sj2 = sj2array[ij2];
4324  {
4325  IkReal evalcond[8];
4326  IkReal x148 = IKsin(j2);
4327  IkReal x149 = IKcos(j2);
4328  if ((((1.0) +
4329  (((-1.0) * (gconst12 * gconst12))))) <
4330  -0.00001)
4331  continue;
4332  IkReal x150 = IKsqrt(
4333  ((1.0) +
4334  (((-1.0) * (gconst12 * gconst12)))));
4335  evalcond[0] = x149;
4336  evalcond[1] = ((-1.0) * x148);
4337  evalcond[2] = (((gconst12 * x148)) + new_r10);
4338  evalcond[3] = (((gconst12 * x149)) + new_r11);
4339  evalcond[4] = (((x148 * x150)) + new_r00);
4340  evalcond[5] = (((x149 * x150)) + new_r01);
4341  evalcond[6] = (((new_r00 * x150)) + x148 +
4342  ((gconst12 * new_r10)));
4343  evalcond[7] = (((new_r01 * x150)) + x149 +
4344  ((gconst12 * new_r11)));
4345  if (IKabs(evalcond[0]) >
4346  IKFAST_EVALCOND_THRESH ||
4347  IKabs(evalcond[1]) >
4348  IKFAST_EVALCOND_THRESH ||
4349  IKabs(evalcond[2]) >
4350  IKFAST_EVALCOND_THRESH ||
4351  IKabs(evalcond[3]) >
4352  IKFAST_EVALCOND_THRESH ||
4353  IKabs(evalcond[4]) >
4354  IKFAST_EVALCOND_THRESH ||
4355  IKabs(evalcond[5]) >
4356  IKFAST_EVALCOND_THRESH ||
4357  IKabs(evalcond[6]) >
4358  IKFAST_EVALCOND_THRESH ||
4359  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4360  {
4361  continue;
4362  }
4363  }
4364 
4365  {
4366  std::vector<IkSingleDOFSolutionBase<IkReal> >
4367  vinfos(7);
4368  vinfos[0].jointtype = 1;
4369  vinfos[0].foffset = j0;
4370  vinfos[0].indices[0] = _ij0[0];
4371  vinfos[0].indices[1] = _ij0[1];
4372  vinfos[0].maxsolutions = _nj0;
4373  vinfos[1].jointtype = 1;
4374  vinfos[1].foffset = j1;
4375  vinfos[1].indices[0] = _ij1[0];
4376  vinfos[1].indices[1] = _ij1[1];
4377  vinfos[1].maxsolutions = _nj1;
4378  vinfos[2].jointtype = 1;
4379  vinfos[2].foffset = j2;
4380  vinfos[2].indices[0] = _ij2[0];
4381  vinfos[2].indices[1] = _ij2[1];
4382  vinfos[2].maxsolutions = _nj2;
4383  vinfos[3].jointtype = 1;
4384  vinfos[3].foffset = j3;
4385  vinfos[3].indices[0] = _ij3[0];
4386  vinfos[3].indices[1] = _ij3[1];
4387  vinfos[3].maxsolutions = _nj3;
4388  vinfos[4].jointtype = 1;
4389  vinfos[4].foffset = j4;
4390  vinfos[4].indices[0] = _ij4[0];
4391  vinfos[4].indices[1] = _ij4[1];
4392  vinfos[4].maxsolutions = _nj4;
4393  vinfos[5].jointtype = 1;
4394  vinfos[5].foffset = j5;
4395  vinfos[5].indices[0] = _ij5[0];
4396  vinfos[5].indices[1] = _ij5[1];
4397  vinfos[5].maxsolutions = _nj5;
4398  vinfos[6].jointtype = 1;
4399  vinfos[6].foffset = j6;
4400  vinfos[6].indices[0] = _ij6[0];
4401  vinfos[6].indices[1] = _ij6[1];
4402  vinfos[6].maxsolutions = _nj6;
4403  std::vector<int> vfree(0);
4404  solutions.AddSolution(vinfos, vfree);
4405  }
4406  }
4407  }
4408  }
4409  else
4410  {
4411  {
4412  IkReal j2array[1], cj2array[1], sj2array[1];
4413  bool j2valid[1] = { false };
4414  _nj2 = 1;
4415  CheckValue<IkReal> x151 =
4416  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4417  IkReal(((-1.0) * new_r11)),
4418  IKFAST_ATAN2_MAGTHRESH);
4419  if (!x151.valid)
4420  {
4421  continue;
4422  }
4423  CheckValue<IkReal> x152 =
4424  IKPowWithIntegerCheck(IKsign(gconst12), -1);
4425  if (!x152.valid)
4426  {
4427  continue;
4428  }
4429  j2array[0] = ((-1.5707963267949) + (x151.value) +
4430  (((1.5707963267949) * (x152.value))));
4431  sj2array[0] = IKsin(j2array[0]);
4432  cj2array[0] = IKcos(j2array[0]);
4433  if (j2array[0] > IKPI)
4434  {
4435  j2array[0] -= IK2PI;
4436  }
4437  else if (j2array[0] < -IKPI)
4438  {
4439  j2array[0] += IK2PI;
4440  }
4441  j2valid[0] = true;
4442  for (int ij2 = 0; ij2 < 1; ++ij2)
4443  {
4444  if (!j2valid[ij2])
4445  {
4446  continue;
4447  }
4448  _ij2[0] = ij2;
4449  _ij2[1] = -1;
4450  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4451  {
4452  if (j2valid[iij2] &&
4453  IKabs(cj2array[ij2] - cj2array[iij2]) <
4454  IKFAST_SOLUTION_THRESH &&
4455  IKabs(sj2array[ij2] - sj2array[iij2]) <
4456  IKFAST_SOLUTION_THRESH)
4457  {
4458  j2valid[iij2] = false;
4459  _ij2[1] = iij2;
4460  break;
4461  }
4462  }
4463  j2 = j2array[ij2];
4464  cj2 = cj2array[ij2];
4465  sj2 = sj2array[ij2];
4466  {
4467  IkReal evalcond[8];
4468  IkReal x153 = IKsin(j2);
4469  IkReal x154 = IKcos(j2);
4470  if ((((1.0) +
4471  (((-1.0) * (gconst12 * gconst12))))) <
4472  -0.00001)
4473  continue;
4474  IkReal x155 = IKsqrt(
4475  ((1.0) +
4476  (((-1.0) * (gconst12 * gconst12)))));
4477  evalcond[0] = x154;
4478  evalcond[1] = ((-1.0) * x153);
4479  evalcond[2] = (((gconst12 * x153)) + new_r10);
4480  evalcond[3] = (((gconst12 * x154)) + new_r11);
4481  evalcond[4] = (((x153 * x155)) + new_r00);
4482  evalcond[5] = (((x154 * x155)) + new_r01);
4483  evalcond[6] = (((new_r00 * x155)) + x153 +
4484  ((gconst12 * new_r10)));
4485  evalcond[7] = (((new_r01 * x155)) + x154 +
4486  ((gconst12 * new_r11)));
4487  if (IKabs(evalcond[0]) >
4488  IKFAST_EVALCOND_THRESH ||
4489  IKabs(evalcond[1]) >
4490  IKFAST_EVALCOND_THRESH ||
4491  IKabs(evalcond[2]) >
4492  IKFAST_EVALCOND_THRESH ||
4493  IKabs(evalcond[3]) >
4494  IKFAST_EVALCOND_THRESH ||
4495  IKabs(evalcond[4]) >
4496  IKFAST_EVALCOND_THRESH ||
4497  IKabs(evalcond[5]) >
4498  IKFAST_EVALCOND_THRESH ||
4499  IKabs(evalcond[6]) >
4500  IKFAST_EVALCOND_THRESH ||
4501  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4502  {
4503  continue;
4504  }
4505  }
4506 
4507  {
4508  std::vector<IkSingleDOFSolutionBase<IkReal> >
4509  vinfos(7);
4510  vinfos[0].jointtype = 1;
4511  vinfos[0].foffset = j0;
4512  vinfos[0].indices[0] = _ij0[0];
4513  vinfos[0].indices[1] = _ij0[1];
4514  vinfos[0].maxsolutions = _nj0;
4515  vinfos[1].jointtype = 1;
4516  vinfos[1].foffset = j1;
4517  vinfos[1].indices[0] = _ij1[0];
4518  vinfos[1].indices[1] = _ij1[1];
4519  vinfos[1].maxsolutions = _nj1;
4520  vinfos[2].jointtype = 1;
4521  vinfos[2].foffset = j2;
4522  vinfos[2].indices[0] = _ij2[0];
4523  vinfos[2].indices[1] = _ij2[1];
4524  vinfos[2].maxsolutions = _nj2;
4525  vinfos[3].jointtype = 1;
4526  vinfos[3].foffset = j3;
4527  vinfos[3].indices[0] = _ij3[0];
4528  vinfos[3].indices[1] = _ij3[1];
4529  vinfos[3].maxsolutions = _nj3;
4530  vinfos[4].jointtype = 1;
4531  vinfos[4].foffset = j4;
4532  vinfos[4].indices[0] = _ij4[0];
4533  vinfos[4].indices[1] = _ij4[1];
4534  vinfos[4].maxsolutions = _nj4;
4535  vinfos[5].jointtype = 1;
4536  vinfos[5].foffset = j5;
4537  vinfos[5].indices[0] = _ij5[0];
4538  vinfos[5].indices[1] = _ij5[1];
4539  vinfos[5].maxsolutions = _nj5;
4540  vinfos[6].jointtype = 1;
4541  vinfos[6].foffset = j6;
4542  vinfos[6].indices[0] = _ij6[0];
4543  vinfos[6].indices[1] = _ij6[1];
4544  vinfos[6].maxsolutions = _nj6;
4545  std::vector<int> vfree(0);
4546  solutions.AddSolution(vinfos, vfree);
4547  }
4548  }
4549  }
4550  }
4551  }
4552  }
4553  } while (0);
4554  if (bgotonextstatement)
4555  {
4556  bool bgotonextstatement = true;
4557  do
4558  {
4559  IkReal x156 = new_r22 * new_r22;
4560  CheckValue<IkReal> x157 = IKPowWithIntegerCheck(
4561  ((1.0) + (((-1.0) * x156))), -1);
4562  if (!x157.valid)
4563  {
4564  continue;
4565  }
4566  if ((((-1.0) * x156 * (x157.value))) < -0.00001)
4567  continue;
4568  IkReal gconst13 =
4569  ((-1.0) * (IKsqrt(((-1.0) * x156 * (x157.value)))));
4570  evalcond[0] =
4571  ((-3.14159265358979) +
4572  (IKfmod(((3.14159265358979) +
4573  (IKabs((cj0 + (((-1.0) * gconst13))))) +
4574  (IKabs(((-1.0) + (IKsign(sj0)))))),
4575  6.28318530717959)));
4576  if (IKabs(evalcond[0]) < 0.0000050000000000)
4577  {
4578  bgotonextstatement = false;
4579  {
4580  IkReal j2eval[1];
4581  IkReal x158 = new_r22 * new_r22;
4582  new_r02 = 0;
4583  new_r12 = 0;
4584  new_r20 = 0;
4585  new_r21 = 0;
4586  if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4587  -0.00001)
4588  continue;
4589  sj0 = IKsqrt(
4590  ((1.0) + (((-1.0) * (gconst13 * gconst13)))));
4591  cj0 = gconst13;
4592  if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4593  (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4594  continue;
4595  j0 = IKacos(gconst13);
4596  CheckValue<IkReal> x159 = IKPowWithIntegerCheck(
4597  ((1.0) + (((-1.0) * x158))), -1);
4598  if (!x159.valid)
4599  {
4600  continue;
4601  }
4602  if ((((-1.0) * x158 * (x159.value))) < -0.00001)
4603  continue;
4604  IkReal gconst13 =
4605  ((-1.0) *
4606  (IKsqrt(((-1.0) * x158 * (x159.value)))));
4607  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4608  if (IKabs(j2eval[0]) < 0.0000010000000000)
4609  {
4610  {
4611  IkReal j2array[1], cj2array[1], sj2array[1];
4612  bool j2valid[1] = { false };
4613  _nj2 = 1;
4614  CheckValue<IkReal> x160 =
4615  IKPowWithIntegerCheck(gconst13, -1);
4616  if (!x160.valid)
4617  {
4618  continue;
4619  }
4620  if ((((1.0) +
4621  (((-1.0) * (gconst13 * gconst13))))) <
4622  -0.00001)
4623  continue;
4624  if (IKabs(((-1.0) * new_r10 * (x160.value))) <
4625  IKFAST_ATAN2_MAGTHRESH &&
4626  IKabs((
4627  (((-1.0) * gconst13 * new_r11)) +
4628  ((new_r01 *
4629  (IKsqrt(((1.0) + (((-1.0) *
4630  (gconst13 *
4631  gconst13)))))))))) <
4632  IKFAST_ATAN2_MAGTHRESH &&
4633  IKabs(
4634  IKsqr(((-1.0) * new_r10 * (x160.value))) +
4635  IKsqr(((((-1.0) * gconst13 * new_r11)) +
4636  ((new_r01 *
4637  (IKsqrt(((1.0) +
4638  (((-1.0) *
4639  (gconst13 *
4640  gconst13)))))))))) -
4641  1) <= IKFAST_SINCOS_THRESH)
4642  continue;
4643  j2array[0] = IKatan2(
4644  ((-1.0) * new_r10 * (x160.value)),
4645  ((((-1.0) * gconst13 * new_r11)) +
4646  ((new_r01 *
4647  (IKsqrt(((1.0) +
4648  (((-1.0) *
4649  (gconst13 * gconst13))))))))));
4650  sj2array[0] = IKsin(j2array[0]);
4651  cj2array[0] = IKcos(j2array[0]);
4652  if (j2array[0] > IKPI)
4653  {
4654  j2array[0] -= IK2PI;
4655  }
4656  else if (j2array[0] < -IKPI)
4657  {
4658  j2array[0] += IK2PI;
4659  }
4660  j2valid[0] = true;
4661  for (int ij2 = 0; ij2 < 1; ++ij2)
4662  {
4663  if (!j2valid[ij2])
4664  {
4665  continue;
4666  }
4667  _ij2[0] = ij2;
4668  _ij2[1] = -1;
4669  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4670  {
4671  if (j2valid[iij2] &&
4672  IKabs(cj2array[ij2] - cj2array[iij2]) <
4673  IKFAST_SOLUTION_THRESH &&
4674  IKabs(sj2array[ij2] - sj2array[iij2]) <
4675  IKFAST_SOLUTION_THRESH)
4676  {
4677  j2valid[iij2] = false;
4678  _ij2[1] = iij2;
4679  break;
4680  }
4681  }
4682  j2 = j2array[ij2];
4683  cj2 = cj2array[ij2];
4684  sj2 = sj2array[ij2];
4685  {
4686  IkReal evalcond[8];
4687  IkReal x161 = IKsin(j2);
4688  IkReal x162 = IKcos(j2);
4689  if ((((1.0) +
4690  (((-1.0) * (gconst13 * gconst13))))) <
4691  -0.00001)
4692  continue;
4693  IkReal x163 = IKsqrt(
4694  ((1.0) +
4695  (((-1.0) * (gconst13 * gconst13)))));
4696  IkReal x164 = ((1.0) * x163);
4697  evalcond[0] = x162;
4698  evalcond[1] = ((-1.0) * x161);
4699  evalcond[2] = (new_r10 + ((gconst13 * x161)));
4700  evalcond[3] = (new_r11 + ((gconst13 * x162)));
4701  evalcond[4] =
4702  ((((-1.0) * x161 * x164)) + new_r00);
4703  evalcond[5] =
4704  ((((-1.0) * x162 * x164)) + new_r01);
4705  evalcond[6] = ((((-1.0) * new_r00 * x164)) +
4706  x161 + ((gconst13 * new_r10)));
4707  evalcond[7] = (x162 + ((gconst13 * new_r11)) +
4708  (((-1.0) * new_r01 * x164)));
4709  if (IKabs(evalcond[0]) >
4710  IKFAST_EVALCOND_THRESH ||
4711  IKabs(evalcond[1]) >
4712  IKFAST_EVALCOND_THRESH ||
4713  IKabs(evalcond[2]) >
4714  IKFAST_EVALCOND_THRESH ||
4715  IKabs(evalcond[3]) >
4716  IKFAST_EVALCOND_THRESH ||
4717  IKabs(evalcond[4]) >
4718  IKFAST_EVALCOND_THRESH ||
4719  IKabs(evalcond[5]) >
4720  IKFAST_EVALCOND_THRESH ||
4721  IKabs(evalcond[6]) >
4722  IKFAST_EVALCOND_THRESH ||
4723  IKabs(evalcond[7]) >
4724  IKFAST_EVALCOND_THRESH)
4725  {
4726  continue;
4727  }
4728  }
4729 
4730  {
4731  std::vector<IkSingleDOFSolutionBase<IkReal> >
4732  vinfos(7);
4733  vinfos[0].jointtype = 1;
4734  vinfos[0].foffset = j0;
4735  vinfos[0].indices[0] = _ij0[0];
4736  vinfos[0].indices[1] = _ij0[1];
4737  vinfos[0].maxsolutions = _nj0;
4738  vinfos[1].jointtype = 1;
4739  vinfos[1].foffset = j1;
4740  vinfos[1].indices[0] = _ij1[0];
4741  vinfos[1].indices[1] = _ij1[1];
4742  vinfos[1].maxsolutions = _nj1;
4743  vinfos[2].jointtype = 1;
4744  vinfos[2].foffset = j2;
4745  vinfos[2].indices[0] = _ij2[0];
4746  vinfos[2].indices[1] = _ij2[1];
4747  vinfos[2].maxsolutions = _nj2;
4748  vinfos[3].jointtype = 1;
4749  vinfos[3].foffset = j3;
4750  vinfos[3].indices[0] = _ij3[0];
4751  vinfos[3].indices[1] = _ij3[1];
4752  vinfos[3].maxsolutions = _nj3;
4753  vinfos[4].jointtype = 1;
4754  vinfos[4].foffset = j4;
4755  vinfos[4].indices[0] = _ij4[0];
4756  vinfos[4].indices[1] = _ij4[1];
4757  vinfos[4].maxsolutions = _nj4;
4758  vinfos[5].jointtype = 1;
4759  vinfos[5].foffset = j5;
4760  vinfos[5].indices[0] = _ij5[0];
4761  vinfos[5].indices[1] = _ij5[1];
4762  vinfos[5].maxsolutions = _nj5;
4763  vinfos[6].jointtype = 1;
4764  vinfos[6].foffset = j6;
4765  vinfos[6].indices[0] = _ij6[0];
4766  vinfos[6].indices[1] = _ij6[1];
4767  vinfos[6].maxsolutions = _nj6;
4768  std::vector<int> vfree(0);
4769  solutions.AddSolution(vinfos, vfree);
4770  }
4771  }
4772  }
4773  }
4774  else
4775  {
4776  {
4777  IkReal j2array[1], cj2array[1], sj2array[1];
4778  bool j2valid[1] = { false };
4779  _nj2 = 1;
4780  CheckValue<IkReal> x165 =
4781  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4782  IkReal(((-1.0) * new_r11)),
4783  IKFAST_ATAN2_MAGTHRESH);
4784  if (!x165.valid)
4785  {
4786  continue;
4787  }
4788  CheckValue<IkReal> x166 =
4789  IKPowWithIntegerCheck(IKsign(gconst13), -1);
4790  if (!x166.valid)
4791  {
4792  continue;
4793  }
4794  j2array[0] =
4795  ((-1.5707963267949) + (x165.value) +
4796  (((1.5707963267949) * (x166.value))));
4797  sj2array[0] = IKsin(j2array[0]);
4798  cj2array[0] = IKcos(j2array[0]);
4799  if (j2array[0] > IKPI)
4800  {
4801  j2array[0] -= IK2PI;
4802  }
4803  else if (j2array[0] < -IKPI)
4804  {
4805  j2array[0] += IK2PI;
4806  }
4807  j2valid[0] = true;
4808  for (int ij2 = 0; ij2 < 1; ++ij2)
4809  {
4810  if (!j2valid[ij2])
4811  {
4812  continue;
4813  }
4814  _ij2[0] = ij2;
4815  _ij2[1] = -1;
4816  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4817  {
4818  if (j2valid[iij2] &&
4819  IKabs(cj2array[ij2] - cj2array[iij2]) <
4820  IKFAST_SOLUTION_THRESH &&
4821  IKabs(sj2array[ij2] - sj2array[iij2]) <
4822  IKFAST_SOLUTION_THRESH)
4823  {
4824  j2valid[iij2] = false;
4825  _ij2[1] = iij2;
4826  break;
4827  }
4828  }
4829  j2 = j2array[ij2];
4830  cj2 = cj2array[ij2];
4831  sj2 = sj2array[ij2];
4832  {
4833  IkReal evalcond[8];
4834  IkReal x167 = IKsin(j2);
4835  IkReal x168 = IKcos(j2);
4836  if ((((1.0) +
4837  (((-1.0) * (gconst13 * gconst13))))) <
4838  -0.00001)
4839  continue;
4840  IkReal x169 = IKsqrt(
4841  ((1.0) +
4842  (((-1.0) * (gconst13 * gconst13)))));
4843  IkReal x170 = ((1.0) * x169);
4844  evalcond[0] = x168;
4845  evalcond[1] = ((-1.0) * x167);
4846  evalcond[2] = (new_r10 + ((gconst13 * x167)));
4847  evalcond[3] = (new_r11 + ((gconst13 * x168)));
4848  evalcond[4] =
4849  (new_r00 + (((-1.0) * x167 * x170)));
4850  evalcond[5] =
4851  ((((-1.0) * x168 * x170)) + new_r01);
4852  evalcond[6] = (x167 + ((gconst13 * new_r10)) +
4853  (((-1.0) * new_r00 * x170)));
4854  evalcond[7] = (x168 + ((gconst13 * new_r11)) +
4855  (((-1.0) * new_r01 * x170)));
4856  if (IKabs(evalcond[0]) >
4857  IKFAST_EVALCOND_THRESH ||
4858  IKabs(evalcond[1]) >
4859  IKFAST_EVALCOND_THRESH ||
4860  IKabs(evalcond[2]) >
4861  IKFAST_EVALCOND_THRESH ||
4862  IKabs(evalcond[3]) >
4863  IKFAST_EVALCOND_THRESH ||
4864  IKabs(evalcond[4]) >
4865  IKFAST_EVALCOND_THRESH ||
4866  IKabs(evalcond[5]) >
4867  IKFAST_EVALCOND_THRESH ||
4868  IKabs(evalcond[6]) >
4869  IKFAST_EVALCOND_THRESH ||
4870  IKabs(evalcond[7]) >
4871  IKFAST_EVALCOND_THRESH)
4872  {
4873  continue;
4874  }
4875  }
4876 
4877  {
4878  std::vector<IkSingleDOFSolutionBase<IkReal> >
4879  vinfos(7);
4880  vinfos[0].jointtype = 1;
4881  vinfos[0].foffset = j0;
4882  vinfos[0].indices[0] = _ij0[0];
4883  vinfos[0].indices[1] = _ij0[1];
4884  vinfos[0].maxsolutions = _nj0;
4885  vinfos[1].jointtype = 1;
4886  vinfos[1].foffset = j1;
4887  vinfos[1].indices[0] = _ij1[0];
4888  vinfos[1].indices[1] = _ij1[1];
4889  vinfos[1].maxsolutions = _nj1;
4890  vinfos[2].jointtype = 1;
4891  vinfos[2].foffset = j2;
4892  vinfos[2].indices[0] = _ij2[0];
4893  vinfos[2].indices[1] = _ij2[1];
4894  vinfos[2].maxsolutions = _nj2;
4895  vinfos[3].jointtype = 1;
4896  vinfos[3].foffset = j3;
4897  vinfos[3].indices[0] = _ij3[0];
4898  vinfos[3].indices[1] = _ij3[1];
4899  vinfos[3].maxsolutions = _nj3;
4900  vinfos[4].jointtype = 1;
4901  vinfos[4].foffset = j4;
4902  vinfos[4].indices[0] = _ij4[0];
4903  vinfos[4].indices[1] = _ij4[1];
4904  vinfos[4].maxsolutions = _nj4;
4905  vinfos[5].jointtype = 1;
4906  vinfos[5].foffset = j5;
4907  vinfos[5].indices[0] = _ij5[0];
4908  vinfos[5].indices[1] = _ij5[1];
4909  vinfos[5].maxsolutions = _nj5;
4910  vinfos[6].jointtype = 1;
4911  vinfos[6].foffset = j6;
4912  vinfos[6].indices[0] = _ij6[0];
4913  vinfos[6].indices[1] = _ij6[1];
4914  vinfos[6].maxsolutions = _nj6;
4915  std::vector<int> vfree(0);
4916  solutions.AddSolution(vinfos, vfree);
4917  }
4918  }
4919  }
4920  }
4921  }
4922  }
4923  } while (0);
4924  if (bgotonextstatement)
4925  {
4926  bool bgotonextstatement = true;
4927  do
4928  {
4929  IkReal x171 = new_r22 * new_r22;
4930  CheckValue<IkReal> x172 = IKPowWithIntegerCheck(
4931  ((1.0) + (((-1.0) * x171))), -1);
4932  if (!x172.valid)
4933  {
4934  continue;
4935  }
4936  if ((((-1.0) * x171 * (x172.value))) < -0.00001)
4937  continue;
4938  IkReal gconst13 =
4939  ((-1.0) * (IKsqrt(((-1.0) * x171 * (x172.value)))));
4940  evalcond[0] =
4941  ((-3.14159265358979) +
4942  (IKfmod(((3.14159265358979) +
4943  (IKabs(((1.0) + (IKsign(sj0))))) +
4944  (IKabs((cj0 + (((-1.0) * gconst13)))))),
4945  6.28318530717959)));
4946  if (IKabs(evalcond[0]) < 0.0000050000000000)
4947  {
4948  bgotonextstatement = false;
4949  {
4950  IkReal j2eval[1];
4951  IkReal x173 = new_r22 * new_r22;
4952  new_r02 = 0;
4953  new_r12 = 0;
4954  new_r20 = 0;
4955  new_r21 = 0;
4956  if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4957  -0.00001)
4958  continue;
4959  sj0 = ((-1.0) *
4960  (IKsqrt(((1.0) + (((-1.0) * (gconst13 *
4961  gconst13)))))));
4962  cj0 = gconst13;
4963  if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4964  (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4965  continue;
4966  j0 = ((-1.0) * (IKacos(gconst13)));
4967  CheckValue<IkReal> x174 = IKPowWithIntegerCheck(
4968  ((1.0) + (((-1.0) * x173))), -1);
4969  if (!x174.valid)
4970  {
4971  continue;
4972  }
4973  if ((((-1.0) * x173 * (x174.value))) < -0.00001)
4974  continue;
4975  IkReal gconst13 =
4976  ((-1.0) *
4977  (IKsqrt(((-1.0) * x173 * (x174.value)))));
4978  j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4979  if (IKabs(j2eval[0]) < 0.0000010000000000)
4980  {
4981  {
4982  IkReal j2array[1], cj2array[1], sj2array[1];
4983  bool j2valid[1] = { false };
4984  _nj2 = 1;
4985  CheckValue<IkReal> x175 =
4986  IKPowWithIntegerCheck(gconst13, -1);
4987  if (!x175.valid)
4988  {
4989  continue;
4990  }
4991  if ((((1.0) +
4992  (((-1.0) * (gconst13 * gconst13))))) <
4993  -0.00001)
4994  continue;
4995  if (IKabs(((-1.0) * new_r10 * (x175.value))) <
4996  IKFAST_ATAN2_MAGTHRESH &&
4997  IKabs(((((-1.0) * gconst13 * new_r11)) +
4998  (((-1.0) * new_r01 *
4999  (IKsqrt(((1.0) +
5000  (((-1.0) *
5001  (gconst13 *
5002  gconst13)))))))))) <
5003  IKFAST_ATAN2_MAGTHRESH &&
5004  IKabs(IKsqr(((-1.0) * new_r10 *
5005  (x175.value))) +
5006  IKsqr((
5007  (((-1.0) * gconst13 * new_r11)) +
5008  (((-1.0) * new_r01 *
5009  (IKsqrt(((1.0) +
5010  (((-1.0) *
5011  (gconst13 *
5012  gconst13)))))))))) -
5013  1) <= IKFAST_SINCOS_THRESH)
5014  continue;
5015  j2array[0] = IKatan2(
5016  ((-1.0) * new_r10 * (x175.value)),
5017  ((((-1.0) * gconst13 * new_r11)) +
5018  (((-1.0) * new_r01 *
5019  (IKsqrt(((1.0) +
5020  (((-1.0) * (gconst13 *
5021  gconst13))))))))));
5022  sj2array[0] = IKsin(j2array[0]);
5023  cj2array[0] = IKcos(j2array[0]);
5024  if (j2array[0] > IKPI)
5025  {
5026  j2array[0] -= IK2PI;
5027  }
5028  else if (j2array[0] < -IKPI)
5029  {
5030  j2array[0] += IK2PI;
5031  }
5032  j2valid[0] = true;
5033  for (int ij2 = 0; ij2 < 1; ++ij2)
5034  {
5035  if (!j2valid[ij2])
5036  {
5037  continue;
5038  }
5039  _ij2[0] = ij2;
5040  _ij2[1] = -1;
5041  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5042  {
5043  if (j2valid[iij2] &&
5044  IKabs(cj2array[ij2] - cj2array[iij2]) <
5045  IKFAST_SOLUTION_THRESH &&
5046  IKabs(sj2array[ij2] - sj2array[iij2]) <
5047  IKFAST_SOLUTION_THRESH)
5048  {
5049  j2valid[iij2] = false;
5050  _ij2[1] = iij2;
5051  break;
5052  }
5053  }
5054  j2 = j2array[ij2];
5055  cj2 = cj2array[ij2];
5056  sj2 = sj2array[ij2];
5057  {
5058  IkReal evalcond[8];
5059  IkReal x176 = IKsin(j2);
5060  IkReal x177 = IKcos(j2);
5061  if ((((1.0) +
5062  (((-1.0) * (gconst13 * gconst13))))) <
5063  -0.00001)
5064  continue;
5065  IkReal x178 = IKsqrt(
5066  ((1.0) +
5067  (((-1.0) * (gconst13 * gconst13)))));
5068  evalcond[0] = x177;
5069  evalcond[1] = ((-1.0) * x176);
5070  evalcond[2] =
5071  (new_r10 + ((gconst13 * x176)));
5072  evalcond[3] =
5073  (new_r11 + ((gconst13 * x177)));
5074  evalcond[4] = (((x176 * x178)) + new_r00);
5075  evalcond[5] = (new_r01 + ((x177 * x178)));
5076  evalcond[6] = (((new_r00 * x178)) + x176 +
5077  ((gconst13 * new_r10)));
5078  evalcond[7] = (((new_r01 * x178)) + x177 +
5079  ((gconst13 * new_r11)));
5080  if (IKabs(evalcond[0]) >
5081  IKFAST_EVALCOND_THRESH ||
5082  IKabs(evalcond[1]) >
5083  IKFAST_EVALCOND_THRESH ||
5084  IKabs(evalcond[2]) >
5085  IKFAST_EVALCOND_THRESH ||
5086  IKabs(evalcond[3]) >
5087  IKFAST_EVALCOND_THRESH ||
5088  IKabs(evalcond[4]) >
5089  IKFAST_EVALCOND_THRESH ||
5090  IKabs(evalcond[5]) >
5091  IKFAST_EVALCOND_THRESH ||
5092  IKabs(evalcond[6]) >
5093  IKFAST_EVALCOND_THRESH ||
5094  IKabs(evalcond[7]) >
5095  IKFAST_EVALCOND_THRESH)
5096  {
5097  continue;
5098  }
5099  }
5100 
5101  {
5102  std::vector<
5103  IkSingleDOFSolutionBase<IkReal> >
5104  vinfos(7);
5105  vinfos[0].jointtype = 1;
5106  vinfos[0].foffset = j0;
5107  vinfos[0].indices[0] = _ij0[0];
5108  vinfos[0].indices[1] = _ij0[1];
5109  vinfos[0].maxsolutions = _nj0;
5110  vinfos[1].jointtype = 1;
5111  vinfos[1].foffset = j1;
5112  vinfos[1].indices[0] = _ij1[0];
5113  vinfos[1].indices[1] = _ij1[1];
5114  vinfos[1].maxsolutions = _nj1;
5115  vinfos[2].jointtype = 1;
5116  vinfos[2].foffset = j2;
5117  vinfos[2].indices[0] = _ij2[0];
5118  vinfos[2].indices[1] = _ij2[1];
5119  vinfos[2].maxsolutions = _nj2;
5120  vinfos[3].jointtype = 1;
5121  vinfos[3].foffset = j3;
5122  vinfos[3].indices[0] = _ij3[0];
5123  vinfos[3].indices[1] = _ij3[1];
5124  vinfos[3].maxsolutions = _nj3;
5125  vinfos[4].jointtype = 1;
5126  vinfos[4].foffset = j4;
5127  vinfos[4].indices[0] = _ij4[0];
5128  vinfos[4].indices[1] = _ij4[1];
5129  vinfos[4].maxsolutions = _nj4;
5130  vinfos[5].jointtype = 1;
5131  vinfos[5].foffset = j5;
5132  vinfos[5].indices[0] = _ij5[0];
5133  vinfos[5].indices[1] = _ij5[1];
5134  vinfos[5].maxsolutions = _nj5;
5135  vinfos[6].jointtype = 1;
5136  vinfos[6].foffset = j6;
5137  vinfos[6].indices[0] = _ij6[0];
5138  vinfos[6].indices[1] = _ij6[1];
5139  vinfos[6].maxsolutions = _nj6;
5140  std::vector<int> vfree(0);
5141  solutions.AddSolution(vinfos, vfree);
5142  }
5143  }
5144  }
5145  }
5146  else
5147  {
5148  {
5149  IkReal j2array[1], cj2array[1], sj2array[1];
5150  bool j2valid[1] = { false };
5151  _nj2 = 1;
5152  CheckValue<IkReal> x179 =
5153  IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
5154  IkReal(((-1.0) * new_r11)),
5155  IKFAST_ATAN2_MAGTHRESH);
5156  if (!x179.valid)
5157  {
5158  continue;
5159  }
5160  CheckValue<IkReal> x180 =
5161  IKPowWithIntegerCheck(IKsign(gconst13), -1);
5162  if (!x180.valid)
5163  {
5164  continue;
5165  }
5166  j2array[0] =
5167  ((-1.5707963267949) + (x179.value) +
5168  (((1.5707963267949) * (x180.value))));
5169  sj2array[0] = IKsin(j2array[0]);
5170  cj2array[0] = IKcos(j2array[0]);
5171  if (j2array[0] > IKPI)
5172  {
5173  j2array[0] -= IK2PI;
5174  }
5175  else if (j2array[0] < -IKPI)
5176  {
5177  j2array[0] += IK2PI;
5178  }
5179  j2valid[0] = true;
5180  for (int ij2 = 0; ij2 < 1; ++ij2)
5181  {
5182  if (!j2valid[ij2])
5183  {
5184  continue;
5185  }
5186  _ij2[0] = ij2;
5187  _ij2[1] = -1;
5188  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5189  {
5190  if (j2valid[iij2] &&
5191  IKabs(cj2array[ij2] - cj2array[iij2]) <
5192  IKFAST_SOLUTION_THRESH &&
5193  IKabs(sj2array[ij2] - sj2array[iij2]) <
5194  IKFAST_SOLUTION_THRESH)
5195  {
5196  j2valid[iij2] = false;
5197  _ij2[1] = iij2;
5198  break;
5199  }
5200  }
5201  j2 = j2array[ij2];
5202  cj2 = cj2array[ij2];
5203  sj2 = sj2array[ij2];
5204  {
5205  IkReal evalcond[8];
5206  IkReal x181 = IKsin(j2);
5207  IkReal x182 = IKcos(j2);
5208  if ((((1.0) +
5209  (((-1.0) * (gconst13 * gconst13))))) <
5210  -0.00001)
5211  continue;
5212  IkReal x183 = IKsqrt(
5213  ((1.0) +
5214  (((-1.0) * (gconst13 * gconst13)))));
5215  evalcond[0] = x182;
5216  evalcond[1] = ((-1.0) * x181);
5217  evalcond[2] =
5218  (((gconst13 * x181)) + new_r10);
5219  evalcond[3] =
5220  (((gconst13 * x182)) + new_r11);
5221  evalcond[4] = (new_r00 + ((x181 * x183)));
5222  evalcond[5] = (new_r01 + ((x182 * x183)));
5223  evalcond[6] = (((new_r00 * x183)) + x181 +
5224  ((gconst13 * new_r10)));
5225  evalcond[7] = (((new_r01 * x183)) + x182 +
5226  ((gconst13 * new_r11)));
5227  if (IKabs(evalcond[0]) >
5228  IKFAST_EVALCOND_THRESH ||
5229  IKabs(evalcond[1]) >
5230  IKFAST_EVALCOND_THRESH ||
5231  IKabs(evalcond[2]) >
5232  IKFAST_EVALCOND_THRESH ||
5233  IKabs(evalcond[3]) >
5234  IKFAST_EVALCOND_THRESH ||
5235  IKabs(evalcond[4]) >
5236  IKFAST_EVALCOND_THRESH ||
5237  IKabs(evalcond[5]) >
5238  IKFAST_EVALCOND_THRESH ||
5239  IKabs(evalcond[6]) >
5240  IKFAST_EVALCOND_THRESH ||
5241  IKabs(evalcond[7]) >
5242  IKFAST_EVALCOND_THRESH)
5243  {
5244  continue;
5245  }
5246  }
5247 
5248  {
5249  std::vector<
5250  IkSingleDOFSolutionBase<IkReal> >
5251  vinfos(7);
5252  vinfos[0].jointtype = 1;
5253  vinfos[0].foffset = j0;
5254  vinfos[0].indices[0] = _ij0[0];
5255  vinfos[0].indices[1] = _ij0[1];
5256  vinfos[0].maxsolutions = _nj0;
5257  vinfos[1].jointtype = 1;
5258  vinfos[1].foffset = j1;
5259  vinfos[1].indices[0] = _ij1[0];
5260  vinfos[1].indices[1] = _ij1[1];
5261  vinfos[1].maxsolutions = _nj1;
5262  vinfos[2].jointtype = 1;
5263  vinfos[2].foffset = j2;
5264  vinfos[2].indices[0] = _ij2[0];
5265  vinfos[2].indices[1] = _ij2[1];
5266  vinfos[2].maxsolutions = _nj2;
5267  vinfos[3].jointtype = 1;
5268  vinfos[3].foffset = j3;
5269  vinfos[3].indices[0] = _ij3[0];
5270  vinfos[3].indices[1] = _ij3[1];
5271  vinfos[3].maxsolutions = _nj3;
5272  vinfos[4].jointtype = 1;
5273  vinfos[4].foffset = j4;
5274  vinfos[4].indices[0] = _ij4[0];
5275  vinfos[4].indices[1] = _ij4[1];
5276  vinfos[4].maxsolutions = _nj4;
5277  vinfos[5].jointtype = 1;
5278  vinfos[5].foffset = j5;
5279  vinfos[5].indices[0] = _ij5[0];
5280  vinfos[5].indices[1] = _ij5[1];
5281  vinfos[5].maxsolutions = _nj5;
5282  vinfos[6].jointtype = 1;
5283  vinfos[6].foffset = j6;
5284  vinfos[6].indices[0] = _ij6[0];
5285  vinfos[6].indices[1] = _ij6[1];
5286  vinfos[6].maxsolutions = _nj6;
5287  std::vector<int> vfree(0);
5288  solutions.AddSolution(vinfos, vfree);
5289  }
5290  }
5291  }
5292  }
5293  }
5294  }
5295  } while (0);
5296  if (bgotonextstatement)
5297  {
5298  bool bgotonextstatement = true;
5299  do
5300  {
5301  if (1)
5302  {
5303  bgotonextstatement = false;
5304  continue; // branch miss [j2]
5305  }
5306  } while (0);
5307  if (bgotonextstatement)
5308  {
5309  }
5310  }
5311  }
5312  }
5313  }
5314  }
5315  }
5316  }
5317  }
5318  else
5319  {
5320  {
5321  IkReal j2array[1], cj2array[1], sj2array[1];
5322  bool j2valid[1] = { false };
5323  _nj2 = 1;
5324  IkReal x184 = (new_r01 * new_r22);
5325  IkReal x185 = (cj0 * new_r11);
5326  CheckValue<IkReal> x186 = IKPowWithIntegerCheck(cj0, -1);
5327  if (!x186.valid)
5328  {
5329  continue;
5330  }
5331  if (IKabs(((x186.value) *
5332  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5333  (((-1.0) * x184)) + (((-1.0) * new_r10)))))) <
5334  IKFAST_ATAN2_MAGTHRESH &&
5335  IKabs((((new_r01 * sj0)) + (((-1.0) * x185)))) <
5336  IKFAST_ATAN2_MAGTHRESH &&
5337  IKabs(
5338  IKsqr(((x186.value) *
5339  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5340  (((-1.0) * x184)) + (((-1.0) * new_r10)))))) +
5341  IKsqr((((new_r01 * sj0)) + (((-1.0) * x185)))) - 1) <=
5342  IKFAST_SINCOS_THRESH)
5343  continue;
5344  j2array[0] =
5345  IKatan2(((x186.value) *
5346  ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5347  (((-1.0) * x184)) + (((-1.0) * new_r10))))),
5348  (((new_r01 * sj0)) + (((-1.0) * x185))));
5349  sj2array[0] = IKsin(j2array[0]);
5350  cj2array[0] = IKcos(j2array[0]);
5351  if (j2array[0] > IKPI)
5352  {
5353  j2array[0] -= IK2PI;
5354  }
5355  else if (j2array[0] < -IKPI)
5356  {
5357  j2array[0] += IK2PI;
5358  }
5359  j2valid[0] = true;
5360  for (int ij2 = 0; ij2 < 1; ++ij2)
5361  {
5362  if (!j2valid[ij2])
5363  {
5364  continue;
5365  }
5366  _ij2[0] = ij2;
5367  _ij2[1] = -1;
5368  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5369  {
5370  if (j2valid[iij2] &&
5371  IKabs(cj2array[ij2] - cj2array[iij2]) <
5372  IKFAST_SOLUTION_THRESH &&
5373  IKabs(sj2array[ij2] - sj2array[iij2]) <
5374  IKFAST_SOLUTION_THRESH)
5375  {
5376  j2valid[iij2] = false;
5377  _ij2[1] = iij2;
5378  break;
5379  }
5380  }
5381  j2 = j2array[ij2];
5382  cj2 = cj2array[ij2];
5383  sj2 = sj2array[ij2];
5384  {
5385  IkReal evalcond[10];
5386  IkReal x187 = IKcos(j2);
5387  IkReal x188 = IKsin(j2);
5388  IkReal x189 = (cj0 * new_r22);
5389  IkReal x190 = ((1.0) * sj0);
5390  IkReal x191 = (new_r22 * sj0);
5391  IkReal x192 = (new_r22 * x188);
5392  IkReal x193 = ((1.0) * x188);
5393  evalcond[0] =
5394  (x188 + (((-1.0) * new_r00 * x190)) + ((cj0 * new_r10)));
5395  evalcond[1] =
5396  (x187 + (((-1.0) * new_r01 * x190)) + ((cj0 * new_r11)));
5397  evalcond[2] = (((new_r10 * sj0)) + ((new_r22 * x187)) +
5398  ((cj0 * new_r00)));
5399  evalcond[3] = (((new_r10 * x191)) + ((new_r00 * x189)) + x187);
5400  evalcond[4] = (((cj0 * x188)) + ((x187 * x191)) + new_r10);
5401  evalcond[5] =
5402  (((new_r11 * sj0)) + (((-1.0) * x192)) + ((cj0 * new_r01)));
5403  evalcond[6] =
5404  ((((-1.0) * x188 * x190)) + ((x187 * x189)) + new_r00);
5405  evalcond[7] =
5406  (((cj0 * x187)) + (((-1.0) * x190 * x192)) + new_r11);
5407  evalcond[8] = (((new_r11 * x191)) + ((new_r01 * x189)) +
5408  (((-1.0) * x193)));
5409  evalcond[9] = ((((-1.0) * x189 * x193)) +
5410  (((-1.0) * x187 * x190)) + new_r01);
5411  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5412  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5413  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5414  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5415  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5416  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5417  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5418  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5419  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5420  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5421  {
5422  continue;
5423  }
5424  }
5425 
5426  {
5427  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5428  vinfos[0].jointtype = 1;
5429  vinfos[0].foffset = j0;
5430  vinfos[0].indices[0] = _ij0[0];
5431  vinfos[0].indices[1] = _ij0[1];
5432  vinfos[0].maxsolutions = _nj0;
5433  vinfos[1].jointtype = 1;
5434  vinfos[1].foffset = j1;
5435  vinfos[1].indices[0] = _ij1[0];
5436  vinfos[1].indices[1] = _ij1[1];
5437  vinfos[1].maxsolutions = _nj1;
5438  vinfos[2].jointtype = 1;
5439  vinfos[2].foffset = j2;
5440  vinfos[2].indices[0] = _ij2[0];
5441  vinfos[2].indices[1] = _ij2[1];
5442  vinfos[2].maxsolutions = _nj2;
5443  vinfos[3].jointtype = 1;
5444  vinfos[3].foffset = j3;
5445  vinfos[3].indices[0] = _ij3[0];
5446  vinfos[3].indices[1] = _ij3[1];
5447  vinfos[3].maxsolutions = _nj3;
5448  vinfos[4].jointtype = 1;
5449  vinfos[4].foffset = j4;
5450  vinfos[4].indices[0] = _ij4[0];
5451  vinfos[4].indices[1] = _ij4[1];
5452  vinfos[4].maxsolutions = _nj4;
5453  vinfos[5].jointtype = 1;
5454  vinfos[5].foffset = j5;
5455  vinfos[5].indices[0] = _ij5[0];
5456  vinfos[5].indices[1] = _ij5[1];
5457  vinfos[5].maxsolutions = _nj5;
5458  vinfos[6].jointtype = 1;
5459  vinfos[6].foffset = j6;
5460  vinfos[6].indices[0] = _ij6[0];
5461  vinfos[6].indices[1] = _ij6[1];
5462  vinfos[6].maxsolutions = _nj6;
5463  std::vector<int> vfree(0);
5464  solutions.AddSolution(vinfos, vfree);
5465  }
5466  }
5467  }
5468  }
5469  }
5470  }
5471  else
5472  {
5473  {
5474  IkReal j2array[1], cj2array[1], sj2array[1];
5475  bool j2valid[1] = { false };
5476  _nj2 = 1;
5477  IkReal x194 = ((1.0) * cj0);
5478  CheckValue<IkReal> x195 = IKPowWithIntegerCheck(new_r22, -1);
5479  if (!x195.valid)
5480  {
5481  continue;
5482  }
5483  if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) <
5484  IKFAST_ATAN2_MAGTHRESH &&
5485  IKabs(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5486  (((-1.0) * new_r00 * x194)))))) <
5487  IKFAST_ATAN2_MAGTHRESH &&
5488  IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) +
5489  IKsqr(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5490  (((-1.0) * new_r00 * x194)))))) -
5491  1) <= IKFAST_SINCOS_THRESH)
5492  continue;
5493  j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194))),
5494  ((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5495  (((-1.0) * new_r00 * x194))))));
5496  sj2array[0] = IKsin(j2array[0]);
5497  cj2array[0] = IKcos(j2array[0]);
5498  if (j2array[0] > IKPI)
5499  {
5500  j2array[0] -= IK2PI;
5501  }
5502  else if (j2array[0] < -IKPI)
5503  {
5504  j2array[0] += IK2PI;
5505  }
5506  j2valid[0] = true;
5507  for (int ij2 = 0; ij2 < 1; ++ij2)
5508  {
5509  if (!j2valid[ij2])
5510  {
5511  continue;
5512  }
5513  _ij2[0] = ij2;
5514  _ij2[1] = -1;
5515  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5516  {
5517  if (j2valid[iij2] &&
5518  IKabs(cj2array[ij2] - cj2array[iij2]) <
5519  IKFAST_SOLUTION_THRESH &&
5520  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5521  {
5522  j2valid[iij2] = false;
5523  _ij2[1] = iij2;
5524  break;
5525  }
5526  }
5527  j2 = j2array[ij2];
5528  cj2 = cj2array[ij2];
5529  sj2 = sj2array[ij2];
5530  {
5531  IkReal evalcond[10];
5532  IkReal x196 = IKcos(j2);
5533  IkReal x197 = IKsin(j2);
5534  IkReal x198 = (cj0 * new_r22);
5535  IkReal x199 = ((1.0) * sj0);
5536  IkReal x200 = (new_r22 * sj0);
5537  IkReal x201 = (new_r22 * x197);
5538  IkReal x202 = ((1.0) * x197);
5539  evalcond[0] =
5540  (x197 + (((-1.0) * new_r00 * x199)) + ((cj0 * new_r10)));
5541  evalcond[1] =
5542  (x196 + (((-1.0) * new_r01 * x199)) + ((cj0 * new_r11)));
5543  evalcond[2] =
5544  (((new_r10 * sj0)) + ((new_r22 * x196)) + ((cj0 * new_r00)));
5545  evalcond[3] = (((new_r00 * x198)) + ((new_r10 * x200)) + x196);
5546  evalcond[4] = (((x196 * x200)) + ((cj0 * x197)) + new_r10);
5547  evalcond[5] =
5548  (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x201)));
5549  evalcond[6] =
5550  ((((-1.0) * x197 * x199)) + ((x196 * x198)) + new_r00);
5551  evalcond[7] = (((cj0 * x196)) + (((-1.0) * x199 * x201)) + new_r11);
5552  evalcond[8] =
5553  (((new_r01 * x198)) + ((new_r11 * x200)) + (((-1.0) * x202)));
5554  evalcond[9] =
5555  ((((-1.0) * x196 * x199)) + (((-1.0) * x198 * x202)) + new_r01);
5556  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5557  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5558  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5559  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5560  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5561  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5562  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5563  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5564  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5565  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5566  {
5567  continue;
5568  }
5569  }
5570 
5571  {
5572  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5573  vinfos[0].jointtype = 1;
5574  vinfos[0].foffset = j0;
5575  vinfos[0].indices[0] = _ij0[0];
5576  vinfos[0].indices[1] = _ij0[1];
5577  vinfos[0].maxsolutions = _nj0;
5578  vinfos[1].jointtype = 1;
5579  vinfos[1].foffset = j1;
5580  vinfos[1].indices[0] = _ij1[0];
5581  vinfos[1].indices[1] = _ij1[1];
5582  vinfos[1].maxsolutions = _nj1;
5583  vinfos[2].jointtype = 1;
5584  vinfos[2].foffset = j2;
5585  vinfos[2].indices[0] = _ij2[0];
5586  vinfos[2].indices[1] = _ij2[1];
5587  vinfos[2].maxsolutions = _nj2;
5588  vinfos[3].jointtype = 1;
5589  vinfos[3].foffset = j3;
5590  vinfos[3].indices[0] = _ij3[0];
5591  vinfos[3].indices[1] = _ij3[1];
5592  vinfos[3].maxsolutions = _nj3;
5593  vinfos[4].jointtype = 1;
5594  vinfos[4].foffset = j4;
5595  vinfos[4].indices[0] = _ij4[0];
5596  vinfos[4].indices[1] = _ij4[1];
5597  vinfos[4].maxsolutions = _nj4;
5598  vinfos[5].jointtype = 1;
5599  vinfos[5].foffset = j5;
5600  vinfos[5].indices[0] = _ij5[0];
5601  vinfos[5].indices[1] = _ij5[1];
5602  vinfos[5].maxsolutions = _nj5;
5603  vinfos[6].jointtype = 1;
5604  vinfos[6].foffset = j6;
5605  vinfos[6].indices[0] = _ij6[0];
5606  vinfos[6].indices[1] = _ij6[1];
5607  vinfos[6].maxsolutions = _nj6;
5608  std::vector<int> vfree(0);
5609  solutions.AddSolution(vinfos, vfree);
5610  }
5611  }
5612  }
5613  }
5614  }
5615  }
5616  else
5617  {
5618  {
5619  IkReal j2array[1], cj2array[1], sj2array[1];
5620  bool j2valid[1] = { false };
5621  _nj2 = 1;
5622  IkReal x203 = cj0 * cj0;
5623  IkReal x204 = new_r22 * new_r22;
5624  IkReal x205 = ((1.0) * cj0);
5625  IkReal x206 = (new_r22 * sj0);
5626  CheckValue<IkReal> x207 = IKatan2WithCheck(
5627  IkReal((((new_r11 * x206)) + (((-1.0) * new_r10 * x205)))),
5628  IkReal(((((-1.0) * new_r11 * x205)) + (((-1.0) * new_r10 * x206)))),
5629  IKFAST_ATAN2_MAGTHRESH);
5630  if (!x207.valid)
5631  {
5632  continue;
5633  }
5634  CheckValue<IkReal> x208 = IKPowWithIntegerCheck(
5635  IKsign((x204 + x203 + (((-1.0) * x203 * x204)))), -1);
5636  if (!x208.valid)
5637  {
5638  continue;
5639  }
5640  j2array[0] = ((-1.5707963267949) + (x207.value) +
5641  (((1.5707963267949) * (x208.value))));
5642  sj2array[0] = IKsin(j2array[0]);
5643  cj2array[0] = IKcos(j2array[0]);
5644  if (j2array[0] > IKPI)
5645  {
5646  j2array[0] -= IK2PI;
5647  }
5648  else if (j2array[0] < -IKPI)
5649  {
5650  j2array[0] += IK2PI;
5651  }
5652  j2valid[0] = true;
5653  for (int ij2 = 0; ij2 < 1; ++ij2)
5654  {
5655  if (!j2valid[ij2])
5656  {
5657  continue;
5658  }
5659  _ij2[0] = ij2;
5660  _ij2[1] = -1;
5661  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5662  {
5663  if (j2valid[iij2] &&
5664  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5665  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5666  {
5667  j2valid[iij2] = false;
5668  _ij2[1] = iij2;
5669  break;
5670  }
5671  }
5672  j2 = j2array[ij2];
5673  cj2 = cj2array[ij2];
5674  sj2 = sj2array[ij2];
5675  {
5676  IkReal evalcond[10];
5677  IkReal x209 = IKcos(j2);
5678  IkReal x210 = IKsin(j2);
5679  IkReal x211 = (cj0 * new_r22);
5680  IkReal x212 = ((1.0) * sj0);
5681  IkReal x213 = (new_r22 * sj0);
5682  IkReal x214 = (new_r22 * x210);
5683  IkReal x215 = ((1.0) * x210);
5684  evalcond[0] = (x210 + (((-1.0) * new_r00 * x212)) + ((cj0 * new_r10)));
5685  evalcond[1] = (x209 + (((-1.0) * new_r01 * x212)) + ((cj0 * new_r11)));
5686  evalcond[2] =
5687  (((new_r10 * sj0)) + ((new_r22 * x209)) + ((cj0 * new_r00)));
5688  evalcond[3] = (x209 + ((new_r00 * x211)) + ((new_r10 * x213)));
5689  evalcond[4] = (((cj0 * x210)) + ((x209 * x213)) + new_r10);
5690  evalcond[5] =
5691  (((new_r11 * sj0)) + (((-1.0) * x214)) + ((cj0 * new_r01)));
5692  evalcond[6] = (((x209 * x211)) + (((-1.0) * x210 * x212)) + new_r00);
5693  evalcond[7] = (((cj0 * x209)) + new_r11 + (((-1.0) * x212 * x214)));
5694  evalcond[8] =
5695  (((new_r01 * x211)) + (((-1.0) * x215)) + ((new_r11 * x213)));
5696  evalcond[9] =
5697  ((((-1.0) * x211 * x215)) + new_r01 + (((-1.0) * x209 * x212)));
5698  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5699  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5700  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5701  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5702  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5703  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5704  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5705  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5706  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5707  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5708  {
5709  continue;
5710  }
5711  }
5712 
5713  {
5714  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5715  vinfos[0].jointtype = 1;
5716  vinfos[0].foffset = j0;
5717  vinfos[0].indices[0] = _ij0[0];
5718  vinfos[0].indices[1] = _ij0[1];
5719  vinfos[0].maxsolutions = _nj0;
5720  vinfos[1].jointtype = 1;
5721  vinfos[1].foffset = j1;
5722  vinfos[1].indices[0] = _ij1[0];
5723  vinfos[1].indices[1] = _ij1[1];
5724  vinfos[1].maxsolutions = _nj1;
5725  vinfos[2].jointtype = 1;
5726  vinfos[2].foffset = j2;
5727  vinfos[2].indices[0] = _ij2[0];
5728  vinfos[2].indices[1] = _ij2[1];
5729  vinfos[2].maxsolutions = _nj2;
5730  vinfos[3].jointtype = 1;
5731  vinfos[3].foffset = j3;
5732  vinfos[3].indices[0] = _ij3[0];
5733  vinfos[3].indices[1] = _ij3[1];
5734  vinfos[3].maxsolutions = _nj3;
5735  vinfos[4].jointtype = 1;
5736  vinfos[4].foffset = j4;
5737  vinfos[4].indices[0] = _ij4[0];
5738  vinfos[4].indices[1] = _ij4[1];
5739  vinfos[4].maxsolutions = _nj4;
5740  vinfos[5].jointtype = 1;
5741  vinfos[5].foffset = j5;
5742  vinfos[5].indices[0] = _ij5[0];
5743  vinfos[5].indices[1] = _ij5[1];
5744  vinfos[5].maxsolutions = _nj5;
5745  vinfos[6].jointtype = 1;
5746  vinfos[6].foffset = j6;
5747  vinfos[6].indices[0] = _ij6[0];
5748  vinfos[6].indices[1] = _ij6[1];
5749  vinfos[6].maxsolutions = _nj6;
5750  std::vector<int> vfree(0);
5751  solutions.AddSolution(vinfos, vfree);
5752  }
5753  }
5754  }
5755  }
5756  }
5757  }
5758  }
5759  }
5760  }
5761  } while (0);
5762  if (bgotonextstatement)
5763  {
5764  bool bgotonextstatement = true;
5765  do
5766  {
5767  if (1)
5768  {
5769  bgotonextstatement = false;
5770  continue; // branch miss [j0, j2]
5771  }
5772  } while (0);
5773  if (bgotonextstatement)
5774  {
5775  }
5776  }
5777  }
5778  }
5779  }
5780  }
5781  else
5782  {
5783  {
5784  IkReal j0array[1], cj0array[1], sj0array[1];
5785  bool j0valid[1] = { false };
5786  _nj0 = 1;
5787  CheckValue<IkReal> x217 = IKPowWithIntegerCheck(sj1, -1);
5788  if (!x217.valid)
5789  {
5790  continue;
5791  }
5792  IkReal x216 = x217.value;
5793  CheckValue<IkReal> x218 = IKPowWithIntegerCheck(new_r12, -1);
5794  if (!x218.valid)
5795  {
5796  continue;
5797  }
5798  if (IKabs((x216 * (x218.value) *
5799  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) <
5800  IKFAST_ATAN2_MAGTHRESH &&
5801  IKabs((new_r02 * x216)) < IKFAST_ATAN2_MAGTHRESH &&
5802  IKabs(IKsqr((x216 * (x218.value) *
5803  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) +
5804  IKsqr((new_r02 * x216)) - 1) <= IKFAST_SINCOS_THRESH)
5805  continue;
5806  j0array[0] = IKatan2((x216 * (x218.value) *
5807  (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1)))))),
5808  (new_r02 * x216));
5809  sj0array[0] = IKsin(j0array[0]);
5810  cj0array[0] = IKcos(j0array[0]);
5811  if (j0array[0] > IKPI)
5812  {
5813  j0array[0] -= IK2PI;
5814  }
5815  else if (j0array[0] < -IKPI)
5816  {
5817  j0array[0] += IK2PI;
5818  }
5819  j0valid[0] = true;
5820  for (int ij0 = 0; ij0 < 1; ++ij0)
5821  {
5822  if (!j0valid[ij0])
5823  {
5824  continue;
5825  }
5826  _ij0[0] = ij0;
5827  _ij0[1] = -1;
5828  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
5829  {
5830  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
5831  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
5832  {
5833  j0valid[iij0] = false;
5834  _ij0[1] = iij0;
5835  break;
5836  }
5837  }
5838  j0 = j0array[ij0];
5839  cj0 = cj0array[ij0];
5840  sj0 = sj0array[ij0];
5841  {
5842  IkReal evalcond[8];
5843  IkReal x219 = IKcos(j0);
5844  IkReal x220 = IKsin(j0);
5845  IkReal x221 = ((1.0) * cj1);
5846  IkReal x222 = ((1.0) * sj1);
5847  IkReal x223 = (new_r12 * x220);
5848  IkReal x224 = (new_r02 * x219);
5849  evalcond[0] = ((((-1.0) * x219 * x222)) + new_r02);
5850  evalcond[1] = ((((-1.0) * x220 * x222)) + new_r12);
5851  evalcond[2] = (((new_r12 * x219)) + (((-1.0) * new_r02 * x220)));
5852  evalcond[3] = (x223 + x224 + (((-1.0) * x222)));
5853  evalcond[4] = (((cj1 * x224)) + ((cj1 * x223)) + (((-1.0) * new_r22 * x222)));
5854  evalcond[5] = ((((-1.0) * new_r10 * x220 * x222)) + (((-1.0) * new_r00 * x219 * x222)) +
5855  (((-1.0) * new_r20 * x221)));
5856  evalcond[6] = ((((-1.0) * new_r11 * x220 * x222)) + (((-1.0) * new_r01 * x219 * x222)) +
5857  (((-1.0) * new_r21 * x221)));
5858  evalcond[7] = ((1.0) + (((-1.0) * x222 * x223)) + (((-1.0) * x222 * x224)) +
5859  (((-1.0) * new_r22 * x221)));
5860  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5861  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5862  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5863  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5864  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5865  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5866  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5867  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5868  {
5869  continue;
5870  }
5871  }
5872 
5873  {
5874  IkReal j2eval[3];
5875  j2eval[0] = sj1;
5876  j2eval[1] = IKsign(sj1);
5877  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
5878  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
5879  IKabs(j2eval[2]) < 0.0000010000000000)
5880  {
5881  {
5882  IkReal j2eval[2];
5883  j2eval[0] = sj1;
5884  j2eval[1] = cj0;
5885  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
5886  {
5887  {
5888  IkReal j2eval[3];
5889  j2eval[0] = sj1;
5890  j2eval[1] = cj1;
5891  j2eval[2] = sj0;
5892  if (IKabs(j2eval[0]) < 0.0000010000000000 ||
5893  IKabs(j2eval[1]) < 0.0000010000000000 || IKabs(j2eval[2]) < 0.0000010000000000)
5894  {
5895  {
5896  IkReal evalcond[5];
5897  bool bgotonextstatement = true;
5898  do
5899  {
5900  evalcond[0] =
5901  ((-3.14159265358979) +
5902  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
5903  evalcond[1] = new_r21;
5904  evalcond[2] = new_r02;
5905  evalcond[3] = new_r12;
5906  evalcond[4] = new_r20;
5907  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
5908  IKabs(evalcond[1]) < 0.0000050000000000 &&
5909  IKabs(evalcond[2]) < 0.0000050000000000 &&
5910  IKabs(evalcond[3]) < 0.0000050000000000 &&
5911  IKabs(evalcond[4]) < 0.0000050000000000)
5912  {
5913  bgotonextstatement = false;
5914  {
5915  IkReal j2array[1], cj2array[1], sj2array[1];
5916  bool j2valid[1] = { false };
5917  _nj2 = 1;
5918  IkReal x225 = ((1.0) * cj0);
5919  if (IKabs(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) <
5920  IKFAST_ATAN2_MAGTHRESH &&
5921  IKabs(((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225)))) <
5922  IKFAST_ATAN2_MAGTHRESH &&
5923  IKabs(IKsqr(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) +
5924  IKsqr(((((-1.0) * new_r10 * sj0)) +
5925  (((-1.0) * new_r00 * x225)))) -
5926  1) <= IKFAST_SINCOS_THRESH)
5927  continue;
5928  j2array[0] =
5929  IKatan2(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0))),
5930  ((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225))));
5931  sj2array[0] = IKsin(j2array[0]);
5932  cj2array[0] = IKcos(j2array[0]);
5933  if (j2array[0] > IKPI)
5934  {
5935  j2array[0] -= IK2PI;
5936  }
5937  else if (j2array[0] < -IKPI)
5938  {
5939  j2array[0] += IK2PI;
5940  }
5941  j2valid[0] = true;
5942  for (int ij2 = 0; ij2 < 1; ++ij2)
5943  {
5944  if (!j2valid[ij2])
5945  {
5946  continue;
5947  }
5948  _ij2[0] = ij2;
5949  _ij2[1] = -1;
5950  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5951  {
5952  if (j2valid[iij2] &&
5953  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5954  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5955  {
5956  j2valid[iij2] = false;
5957  _ij2[1] = iij2;
5958  break;
5959  }
5960  }
5961  j2 = j2array[ij2];
5962  cj2 = cj2array[ij2];
5963  sj2 = sj2array[ij2];
5964  {
5965  IkReal evalcond[8];
5966  IkReal x226 = IKcos(j2);
5967  IkReal x227 = IKsin(j2);
5968  IkReal x228 = ((1.0) * sj0);
5969  IkReal x229 = (cj0 * x226);
5970  IkReal x230 = (cj0 * x227);
5971  IkReal x231 = (x227 * x228);
5972  evalcond[0] = (((new_r10 * sj0)) + x226 + ((cj0 * new_r00)));
5973  evalcond[1] =
5974  (x227 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x228)));
5975  evalcond[2] =
5976  ((((-1.0) * new_r01 * x228)) + x226 + ((cj0 * new_r11)));
5977  evalcond[3] = (((sj0 * x226)) + x230 + new_r10);
5978  evalcond[4] =
5979  (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x227)));
5980  evalcond[5] = ((((-1.0) * x231)) + x229 + new_r00);
5981  evalcond[6] = ((((-1.0) * x231)) + x229 + new_r11);
5982  evalcond[7] =
5983  (new_r01 + (((-1.0) * x226 * x228)) + (((-1.0) * x230)));
5984  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5985  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5986  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5987  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5988  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5989  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5990  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5991  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5992  {
5993  continue;
5994  }
5995  }
5996 
5997  {
5998  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5999  vinfos[0].jointtype = 1;
6000  vinfos[0].foffset = j0;
6001  vinfos[0].indices[0] = _ij0[0];
6002  vinfos[0].indices[1] = _ij0[1];
6003  vinfos[0].maxsolutions = _nj0;
6004  vinfos[1].jointtype = 1;
6005  vinfos[1].foffset = j1;
6006  vinfos[1].indices[0] = _ij1[0];
6007  vinfos[1].indices[1] = _ij1[1];
6008  vinfos[1].maxsolutions = _nj1;
6009  vinfos[2].jointtype = 1;
6010  vinfos[2].foffset = j2;
6011  vinfos[2].indices[0] = _ij2[0];
6012  vinfos[2].indices[1] = _ij2[1];
6013  vinfos[2].maxsolutions = _nj2;
6014  vinfos[3].jointtype = 1;
6015  vinfos[3].foffset = j3;
6016  vinfos[3].indices[0] = _ij3[0];
6017  vinfos[3].indices[1] = _ij3[1];
6018  vinfos[3].maxsolutions = _nj3;
6019  vinfos[4].jointtype = 1;
6020  vinfos[4].foffset = j4;
6021  vinfos[4].indices[0] = _ij4[0];
6022  vinfos[4].indices[1] = _ij4[1];
6023  vinfos[4].maxsolutions = _nj4;
6024  vinfos[5].jointtype = 1;
6025  vinfos[5].foffset = j5;
6026  vinfos[5].indices[0] = _ij5[0];
6027  vinfos[5].indices[1] = _ij5[1];
6028  vinfos[5].maxsolutions = _nj5;
6029  vinfos[6].jointtype = 1;
6030  vinfos[6].foffset = j6;
6031  vinfos[6].indices[0] = _ij6[0];
6032  vinfos[6].indices[1] = _ij6[1];
6033  vinfos[6].maxsolutions = _nj6;
6034  std::vector<int> vfree(0);
6035  solutions.AddSolution(vinfos, vfree);
6036  }
6037  }
6038  }
6039  }
6040  } while (0);
6041  if (bgotonextstatement)
6042  {
6043  bool bgotonextstatement = true;
6044  do
6045  {
6046  evalcond[0] =
6047  ((-3.14159265358979) +
6048  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
6049  6.28318530717959)));
6050  evalcond[1] = new_r21;
6051  evalcond[2] = new_r02;
6052  evalcond[3] = new_r12;
6053  evalcond[4] = new_r20;
6054  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6055  IKabs(evalcond[1]) < 0.0000050000000000 &&
6056  IKabs(evalcond[2]) < 0.0000050000000000 &&
6057  IKabs(evalcond[3]) < 0.0000050000000000 &&
6058  IKabs(evalcond[4]) < 0.0000050000000000)
6059  {
6060  bgotonextstatement = false;
6061  {
6062  IkReal j2array[1], cj2array[1], sj2array[1];
6063  bool j2valid[1] = { false };
6064  _nj2 = 1;
6065  IkReal x232 = ((1.0) * cj0);
6066  if (IKabs(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232)))) <
6067  IKFAST_ATAN2_MAGTHRESH &&
6068  IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) <
6069  IKFAST_ATAN2_MAGTHRESH &&
6070  IKabs(IKsqr(((((-1.0) * new_r11 * sj0)) +
6071  (((-1.0) * new_r10 * x232)))) +
6072  IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) -
6073  1) <= IKFAST_SINCOS_THRESH)
6074  continue;
6075  j2array[0] =
6076  IKatan2(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232))),
6077  (((new_r10 * sj0)) + (((-1.0) * new_r11 * x232))));
6078  sj2array[0] = IKsin(j2array[0]);
6079  cj2array[0] = IKcos(j2array[0]);
6080  if (j2array[0] > IKPI)
6081  {
6082  j2array[0] -= IK2PI;
6083  }
6084  else if (j2array[0] < -IKPI)
6085  {
6086  j2array[0] += IK2PI;
6087  }
6088  j2valid[0] = true;
6089  for (int ij2 = 0; ij2 < 1; ++ij2)
6090  {
6091  if (!j2valid[ij2])
6092  {
6093  continue;
6094  }
6095  _ij2[0] = ij2;
6096  _ij2[1] = -1;
6097  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6098  {
6099  if (j2valid[iij2] &&
6100  IKabs(cj2array[ij2] - cj2array[iij2]) <
6101  IKFAST_SOLUTION_THRESH &&
6102  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
6103  {
6104  j2valid[iij2] = false;
6105  _ij2[1] = iij2;
6106  break;
6107  }
6108  }
6109  j2 = j2array[ij2];
6110  cj2 = cj2array[ij2];
6111  sj2 = sj2array[ij2];
6112  {
6113  IkReal evalcond[8];
6114  IkReal x233 = IKsin(j2);
6115  IkReal x234 = IKcos(j2);
6116  IkReal x235 = ((1.0) * sj0);
6117  IkReal x236 = (cj0 * x233);
6118  IkReal x237 = ((1.0) * x234);
6119  IkReal x238 = (x234 * x235);
6120  evalcond[0] = (((new_r11 * sj0)) + x233 + ((cj0 * new_r01)));
6121  evalcond[1] =
6122  (x233 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x235)));
6123  evalcond[2] =
6124  (x234 + (((-1.0) * new_r01 * x235)) + ((cj0 * new_r11)));
6125  evalcond[3] =
6126  (((new_r10 * sj0)) + (((-1.0) * x237)) + ((cj0 * new_r00)));
6127  evalcond[4] = (((sj0 * x233)) + new_r11 + ((cj0 * x234)));
6128  evalcond[5] = ((((-1.0) * x238)) + x236 + new_r10);
6129  evalcond[6] = ((((-1.0) * x238)) + x236 + new_r01);
6130  evalcond[7] =
6131  ((((-1.0) * x233 * x235)) + (((-1.0) * cj0 * x237)) + new_r00);
6132  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6133  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6134  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6135  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6136  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6137  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6138  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6139  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6140  {
6141  continue;
6142  }
6143  }
6144 
6145  {
6146  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6147  vinfos[0].jointtype = 1;
6148  vinfos[0].foffset = j0;
6149  vinfos[0].indices[0] = _ij0[0];
6150  vinfos[0].indices[1] = _ij0[1];
6151  vinfos[0].maxsolutions = _nj0;
6152  vinfos[1].jointtype = 1;
6153  vinfos[1].foffset = j1;
6154  vinfos[1].indices[0] = _ij1[0];
6155  vinfos[1].indices[1] = _ij1[1];
6156  vinfos[1].maxsolutions = _nj1;
6157  vinfos[2].jointtype = 1;
6158  vinfos[2].foffset = j2;
6159  vinfos[2].indices[0] = _ij2[0];
6160  vinfos[2].indices[1] = _ij2[1];
6161  vinfos[2].maxsolutions = _nj2;
6162  vinfos[3].jointtype = 1;
6163  vinfos[3].foffset = j3;
6164  vinfos[3].indices[0] = _ij3[0];
6165  vinfos[3].indices[1] = _ij3[1];
6166  vinfos[3].maxsolutions = _nj3;
6167  vinfos[4].jointtype = 1;
6168  vinfos[4].foffset = j4;
6169  vinfos[4].indices[0] = _ij4[0];
6170  vinfos[4].indices[1] = _ij4[1];
6171  vinfos[4].maxsolutions = _nj4;
6172  vinfos[5].jointtype = 1;
6173  vinfos[5].foffset = j5;
6174  vinfos[5].indices[0] = _ij5[0];
6175  vinfos[5].indices[1] = _ij5[1];
6176  vinfos[5].maxsolutions = _nj5;
6177  vinfos[6].jointtype = 1;
6178  vinfos[6].foffset = j6;
6179  vinfos[6].indices[0] = _ij6[0];
6180  vinfos[6].indices[1] = _ij6[1];
6181  vinfos[6].maxsolutions = _nj6;
6182  std::vector<int> vfree(0);
6183  solutions.AddSolution(vinfos, vfree);
6184  }
6185  }
6186  }
6187  }
6188  } while (0);
6189  if (bgotonextstatement)
6190  {
6191  bool bgotonextstatement = true;
6192  do
6193  {
6194  evalcond[0] =
6195  ((-3.14159265358979) +
6196  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
6197  6.28318530717959)));
6198  evalcond[1] = new_r22;
6199  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6200  IKabs(evalcond[1]) < 0.0000050000000000)
6201  {
6202  bgotonextstatement = false;
6203  {
6204  IkReal j2array[1], cj2array[1], sj2array[1];
6205  bool j2valid[1] = { false };
6206  _nj2 = 1;
6207  if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
6208  IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
6209  IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
6210  IKFAST_SINCOS_THRESH)
6211  continue;
6212  j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
6213  sj2array[0] = IKsin(j2array[0]);
6214  cj2array[0] = IKcos(j2array[0]);
6215  if (j2array[0] > IKPI)
6216  {
6217  j2array[0] -= IK2PI;
6218  }
6219  else if (j2array[0] < -IKPI)
6220  {
6221  j2array[0] += IK2PI;
6222  }
6223  j2valid[0] = true;
6224  for (int ij2 = 0; ij2 < 1; ++ij2)
6225  {
6226  if (!j2valid[ij2])
6227  {
6228  continue;
6229  }
6230  _ij2[0] = ij2;
6231  _ij2[1] = -1;
6232  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6233  {
6234  if (j2valid[iij2] &&
6235  IKabs(cj2array[ij2] - cj2array[iij2]) <
6236  IKFAST_SOLUTION_THRESH &&
6237  IKabs(sj2array[ij2] - sj2array[iij2]) <
6238  IKFAST_SOLUTION_THRESH)
6239  {
6240  j2valid[iij2] = false;
6241  _ij2[1] = iij2;
6242  break;
6243  }
6244  }
6245  j2 = j2array[ij2];
6246  cj2 = cj2array[ij2];
6247  sj2 = sj2array[ij2];
6248  {
6249  IkReal evalcond[8];
6250  IkReal x239 = IKsin(j2);
6251  IkReal x240 = IKcos(j2);
6252  IkReal x241 = ((1.0) * sj0);
6253  evalcond[0] = (x239 + new_r21);
6254  evalcond[1] = ((((-1.0) * x240)) + new_r20);
6255  evalcond[2] = (((new_r02 * x239)) + new_r10);
6256  evalcond[3] = (((cj0 * x240)) + new_r11);
6257  evalcond[4] = (new_r00 + (((-1.0) * x239 * x241)));
6258  evalcond[5] = ((((-1.0) * x240 * x241)) + new_r01);
6259  evalcond[6] =
6260  ((((-1.0) * new_r00 * x241)) + x239 + ((cj0 * new_r10)));
6261  evalcond[7] =
6262  ((((-1.0) * new_r01 * x241)) + x240 + ((cj0 * new_r11)));
6263  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6264  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6265  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6266  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6267  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6268  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6269  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6270  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6271  {
6272  continue;
6273  }
6274  }
6275 
6276  {
6277  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6278  vinfos[0].jointtype = 1;
6279  vinfos[0].foffset = j0;
6280  vinfos[0].indices[0] = _ij0[0];
6281  vinfos[0].indices[1] = _ij0[1];
6282  vinfos[0].maxsolutions = _nj0;
6283  vinfos[1].jointtype = 1;
6284  vinfos[1].foffset = j1;
6285  vinfos[1].indices[0] = _ij1[0];
6286  vinfos[1].indices[1] = _ij1[1];
6287  vinfos[1].maxsolutions = _nj1;
6288  vinfos[2].jointtype = 1;
6289  vinfos[2].foffset = j2;
6290  vinfos[2].indices[0] = _ij2[0];
6291  vinfos[2].indices[1] = _ij2[1];
6292  vinfos[2].maxsolutions = _nj2;
6293  vinfos[3].jointtype = 1;
6294  vinfos[3].foffset = j3;
6295  vinfos[3].indices[0] = _ij3[0];
6296  vinfos[3].indices[1] = _ij3[1];
6297  vinfos[3].maxsolutions = _nj3;
6298  vinfos[4].jointtype = 1;
6299  vinfos[4].foffset = j4;
6300  vinfos[4].indices[0] = _ij4[0];
6301  vinfos[4].indices[1] = _ij4[1];
6302  vinfos[4].maxsolutions = _nj4;
6303  vinfos[5].jointtype = 1;
6304  vinfos[5].foffset = j5;
6305  vinfos[5].indices[0] = _ij5[0];
6306  vinfos[5].indices[1] = _ij5[1];
6307  vinfos[5].maxsolutions = _nj5;
6308  vinfos[6].jointtype = 1;
6309  vinfos[6].foffset = j6;
6310  vinfos[6].indices[0] = _ij6[0];
6311  vinfos[6].indices[1] = _ij6[1];
6312  vinfos[6].maxsolutions = _nj6;
6313  std::vector<int> vfree(0);
6314  solutions.AddSolution(vinfos, vfree);
6315  }
6316  }
6317  }
6318  }
6319  } while (0);
6320  if (bgotonextstatement)
6321  {
6322  bool bgotonextstatement = true;
6323  do
6324  {
6325  evalcond[0] =
6326  ((-3.14159265358979) +
6327  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
6328  6.28318530717959)));
6329  evalcond[1] = new_r22;
6330  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6331  IKabs(evalcond[1]) < 0.0000050000000000)
6332  {
6333  bgotonextstatement = false;
6334  {
6335  IkReal j2array[1], cj2array[1], sj2array[1];
6336  bool j2valid[1] = { false };
6337  _nj2 = 1;
6338  if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
6339  IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
6340  IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
6341  IKFAST_SINCOS_THRESH)
6342  continue;
6343  j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
6344  sj2array[0] = IKsin(j2array[0]);
6345  cj2array[0] = IKcos(j2array[0]);
6346  if (j2array[0] > IKPI)
6347  {
6348  j2array[0] -= IK2PI;
6349  }
6350  else if (j2array[0] < -IKPI)
6351  {
6352  j2array[0] += IK2PI;
6353  }
6354  j2valid[0] = true;
6355  for (int ij2 = 0; ij2 < 1; ++ij2)
6356  {
6357  if (!j2valid[ij2])
6358  {
6359  continue;
6360  }
6361  _ij2[0] = ij2;
6362  _ij2[1] = -1;
6363  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6364  {
6365  if (j2valid[iij2] &&
6366  IKabs(cj2array[ij2] - cj2array[iij2]) <
6367  IKFAST_SOLUTION_THRESH &&
6368  IKabs(sj2array[ij2] - sj2array[iij2]) <
6369  IKFAST_SOLUTION_THRESH)
6370  {
6371  j2valid[iij2] = false;
6372  _ij2[1] = iij2;
6373  break;
6374  }
6375  }
6376  j2 = j2array[ij2];
6377  cj2 = cj2array[ij2];
6378  sj2 = sj2array[ij2];
6379  {
6380  IkReal evalcond[8];
6381  IkReal x242 = IKcos(j2);
6382  IkReal x243 = IKsin(j2);
6383  IkReal x244 = ((1.0) * sj0);
6384  IkReal x245 = ((1.0) * x243);
6385  evalcond[0] = (x242 + new_r20);
6386  evalcond[1] = ((((-1.0) * x245)) + new_r21);
6387  evalcond[2] = (((cj0 * x242)) + new_r11);
6388  evalcond[3] = ((((-1.0) * new_r02 * x245)) + new_r10);
6389  evalcond[4] = ((((-1.0) * x243 * x244)) + new_r00);
6390  evalcond[5] = ((((-1.0) * x242 * x244)) + new_r01);
6391  evalcond[6] =
6392  ((((-1.0) * new_r00 * x244)) + x243 + ((cj0 * new_r10)));
6393  evalcond[7] =
6394  ((((-1.0) * new_r01 * x244)) + x242 + ((cj0 * new_r11)));
6395  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6396  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6397  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6398  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6399  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6400  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6401  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6402  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6403  {
6404  continue;
6405  }
6406  }
6407 
6408  {
6409  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6410  vinfos[0].jointtype = 1;
6411  vinfos[0].foffset = j0;
6412  vinfos[0].indices[0] = _ij0[0];
6413  vinfos[0].indices[1] = _ij0[1];
6414  vinfos[0].maxsolutions = _nj0;
6415  vinfos[1].jointtype = 1;
6416  vinfos[1].foffset = j1;
6417  vinfos[1].indices[0] = _ij1[0];
6418  vinfos[1].indices[1] = _ij1[1];
6419  vinfos[1].maxsolutions = _nj1;
6420  vinfos[2].jointtype = 1;
6421  vinfos[2].foffset = j2;
6422  vinfos[2].indices[0] = _ij2[0];
6423  vinfos[2].indices[1] = _ij2[1];
6424  vinfos[2].maxsolutions = _nj2;
6425  vinfos[3].jointtype = 1;
6426  vinfos[3].foffset = j3;
6427  vinfos[3].indices[0] = _ij3[0];
6428  vinfos[3].indices[1] = _ij3[1];
6429  vinfos[3].maxsolutions = _nj3;
6430  vinfos[4].jointtype = 1;
6431  vinfos[4].foffset = j4;
6432  vinfos[4].indices[0] = _ij4[0];
6433  vinfos[4].indices[1] = _ij4[1];
6434  vinfos[4].maxsolutions = _nj4;
6435  vinfos[5].jointtype = 1;
6436  vinfos[5].foffset = j5;
6437  vinfos[5].indices[0] = _ij5[0];
6438  vinfos[5].indices[1] = _ij5[1];
6439  vinfos[5].maxsolutions = _nj5;
6440  vinfos[6].jointtype = 1;
6441  vinfos[6].foffset = j6;
6442  vinfos[6].indices[0] = _ij6[0];
6443  vinfos[6].indices[1] = _ij6[1];
6444  vinfos[6].maxsolutions = _nj6;
6445  std::vector<int> vfree(0);
6446  solutions.AddSolution(vinfos, vfree);
6447  }
6448  }
6449  }
6450  }
6451  } while (0);
6452  if (bgotonextstatement)
6453  {
6454  bool bgotonextstatement = true;
6455  do
6456  {
6457  evalcond[0] =
6458  ((-3.14159265358979) +
6459  (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
6460  evalcond[1] = new_r12;
6461  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6462  IKabs(evalcond[1]) < 0.0000050000000000)
6463  {
6464  bgotonextstatement = false;
6465  {
6466  IkReal j2array[1], cj2array[1], sj2array[1];
6467  bool j2valid[1] = { false };
6468  _nj2 = 1;
6469  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
6470  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
6471  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) -
6472  1) <= IKFAST_SINCOS_THRESH)
6473  continue;
6474  j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
6475  sj2array[0] = IKsin(j2array[0]);
6476  cj2array[0] = IKcos(j2array[0]);
6477  if (j2array[0] > IKPI)
6478  {
6479  j2array[0] -= IK2PI;
6480  }
6481  else if (j2array[0] < -IKPI)
6482  {
6483  j2array[0] += IK2PI;
6484  }
6485  j2valid[0] = true;
6486  for (int ij2 = 0; ij2 < 1; ++ij2)
6487  {
6488  if (!j2valid[ij2])
6489  {
6490  continue;
6491  }
6492  _ij2[0] = ij2;
6493  _ij2[1] = -1;
6494  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6495  {
6496  if (j2valid[iij2] &&
6497  IKabs(cj2array[ij2] - cj2array[iij2]) <
6498  IKFAST_SOLUTION_THRESH &&
6499  IKabs(sj2array[ij2] - sj2array[iij2]) <
6500  IKFAST_SOLUTION_THRESH)
6501  {
6502  j2valid[iij2] = false;
6503  _ij2[1] = iij2;
6504  break;
6505  }
6506  }
6507  j2 = j2array[ij2];
6508  cj2 = cj2array[ij2];
6509  sj2 = sj2array[ij2];
6510  {
6511  IkReal evalcond[8];
6512  IkReal x246 = IKsin(j2);
6513  IkReal x247 = IKcos(j2);
6514  IkReal x248 = ((1.0) * sj1);
6515  IkReal x249 = ((1.0) * x246);
6516  evalcond[0] = (x246 + new_r10);
6517  evalcond[1] = (x247 + new_r11);
6518  evalcond[2] = (new_r21 + ((sj1 * x246)));
6519  evalcond[3] = (((cj1 * x247)) + new_r00);
6520  evalcond[4] = (new_r20 + (((-1.0) * x247 * x248)));
6521  evalcond[5] = ((((-1.0) * cj1 * x249)) + new_r01);
6522  evalcond[6] =
6523  (((cj1 * new_r00)) + x247 + (((-1.0) * new_r20 * x248)));
6524  evalcond[7] = ((((-1.0) * new_r21 * x248)) +
6525  ((cj1 * new_r01)) + (((-1.0) * x249)));
6526  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6527  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6528  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6529  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6530  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6531  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6532  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6533  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6534  {
6535  continue;
6536  }
6537  }
6538 
6539  {
6540  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6541  vinfos[0].jointtype = 1;
6542  vinfos[0].foffset = j0;
6543  vinfos[0].indices[0] = _ij0[0];
6544  vinfos[0].indices[1] = _ij0[1];
6545  vinfos[0].maxsolutions = _nj0;
6546  vinfos[1].jointtype = 1;
6547  vinfos[1].foffset = j1;
6548  vinfos[1].indices[0] = _ij1[0];
6549  vinfos[1].indices[1] = _ij1[1];
6550  vinfos[1].maxsolutions = _nj1;
6551  vinfos[2].jointtype = 1;
6552  vinfos[2].foffset = j2;
6553  vinfos[2].indices[0] = _ij2[0];
6554  vinfos[2].indices[1] = _ij2[1];
6555  vinfos[2].maxsolutions = _nj2;
6556  vinfos[3].jointtype = 1;
6557  vinfos[3].foffset = j3;
6558  vinfos[3].indices[0] = _ij3[0];
6559  vinfos[3].indices[1] = _ij3[1];
6560  vinfos[3].maxsolutions = _nj3;
6561  vinfos[4].jointtype = 1;
6562  vinfos[4].foffset = j4;
6563  vinfos[4].indices[0] = _ij4[0];
6564  vinfos[4].indices[1] = _ij4[1];
6565  vinfos[4].maxsolutions = _nj4;
6566  vinfos[5].jointtype = 1;
6567  vinfos[5].foffset = j5;
6568  vinfos[5].indices[0] = _ij5[0];
6569  vinfos[5].indices[1] = _ij5[1];
6570  vinfos[5].maxsolutions = _nj5;
6571  vinfos[6].jointtype = 1;
6572  vinfos[6].foffset = j6;
6573  vinfos[6].indices[0] = _ij6[0];
6574  vinfos[6].indices[1] = _ij6[1];
6575  vinfos[6].maxsolutions = _nj6;
6576  std::vector<int> vfree(0);
6577  solutions.AddSolution(vinfos, vfree);
6578  }
6579  }
6580  }
6581  }
6582  } while (0);
6583  if (bgotonextstatement)
6584  {
6585  bool bgotonextstatement = true;
6586  do
6587  {
6588  evalcond[0] = ((-3.14159265358979) +
6589  (IKfmod(((3.14159265358979) +
6590  (IKabs(((-3.14159265358979) + j0)))),
6591  6.28318530717959)));
6592  evalcond[1] = new_r12;
6593  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6594  IKabs(evalcond[1]) < 0.0000050000000000)
6595  {
6596  bgotonextstatement = false;
6597  {
6598  IkReal j2array[1], cj2array[1], sj2array[1];
6599  bool j2valid[1] = { false };
6600  _nj2 = 1;
6601  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
6602  IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
6603  IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <=
6604  IKFAST_SINCOS_THRESH)
6605  continue;
6606  j2array[0] = IKatan2(new_r10, new_r11);
6607  sj2array[0] = IKsin(j2array[0]);
6608  cj2array[0] = IKcos(j2array[0]);
6609  if (j2array[0] > IKPI)
6610  {
6611  j2array[0] -= IK2PI;
6612  }
6613  else if (j2array[0] < -IKPI)
6614  {
6615  j2array[0] += IK2PI;
6616  }
6617  j2valid[0] = true;
6618  for (int ij2 = 0; ij2 < 1; ++ij2)
6619  {
6620  if (!j2valid[ij2])
6621  {
6622  continue;
6623  }
6624  _ij2[0] = ij2;
6625  _ij2[1] = -1;
6626  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6627  {
6628  if (j2valid[iij2] &&
6629  IKabs(cj2array[ij2] - cj2array[iij2]) <
6630  IKFAST_SOLUTION_THRESH &&
6631  IKabs(sj2array[ij2] - sj2array[iij2]) <
6632  IKFAST_SOLUTION_THRESH)
6633  {
6634  j2valid[iij2] = false;
6635  _ij2[1] = iij2;
6636  break;
6637  }
6638  }
6639  j2 = j2array[ij2];
6640  cj2 = cj2array[ij2];
6641  sj2 = sj2array[ij2];
6642  {
6643  IkReal evalcond[8];
6644  IkReal x250 = IKsin(j2);
6645  IkReal x251 = IKcos(j2);
6646  IkReal x252 = ((1.0) * sj1);
6647  IkReal x253 = ((1.0) * new_r00);
6648  IkReal x254 = ((1.0) * new_r01);
6649  IkReal x255 = ((1.0) * x250);
6650  evalcond[0] = (((sj1 * x250)) + new_r21);
6651  evalcond[1] = (x250 + (((-1.0) * new_r10)));
6652  evalcond[2] = (x251 + (((-1.0) * new_r11)));
6653  evalcond[3] = ((((-1.0) * x251 * x252)) + new_r20);
6654  evalcond[4] = (((cj1 * x251)) + (((-1.0) * x253)));
6655  evalcond[5] = ((((-1.0) * cj1 * x255)) + (((-1.0) * x254)));
6656  evalcond[6] = (x251 + (((-1.0) * cj1 * x253)) +
6657  (((-1.0) * new_r20 * x252)));
6658  evalcond[7] = ((((-1.0) * new_r21 * x252)) +
6659  (((-1.0) * cj1 * x254)) + (((-1.0) * x255)));
6660  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6661  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6662  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6663  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6664  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6665  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6666  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6667  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6668  {
6669  continue;
6670  }
6671  }
6672 
6673  {
6674  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6675  vinfos[0].jointtype = 1;
6676  vinfos[0].foffset = j0;
6677  vinfos[0].indices[0] = _ij0[0];
6678  vinfos[0].indices[1] = _ij0[1];
6679  vinfos[0].maxsolutions = _nj0;
6680  vinfos[1].jointtype = 1;
6681  vinfos[1].foffset = j1;
6682  vinfos[1].indices[0] = _ij1[0];
6683  vinfos[1].indices[1] = _ij1[1];
6684  vinfos[1].maxsolutions = _nj1;
6685  vinfos[2].jointtype = 1;
6686  vinfos[2].foffset = j2;
6687  vinfos[2].indices[0] = _ij2[0];
6688  vinfos[2].indices[1] = _ij2[1];
6689  vinfos[2].maxsolutions = _nj2;
6690  vinfos[3].jointtype = 1;
6691  vinfos[3].foffset = j3;
6692  vinfos[3].indices[0] = _ij3[0];
6693  vinfos[3].indices[1] = _ij3[1];
6694  vinfos[3].maxsolutions = _nj3;
6695  vinfos[4].jointtype = 1;
6696  vinfos[4].foffset = j4;
6697  vinfos[4].indices[0] = _ij4[0];
6698  vinfos[4].indices[1] = _ij4[1];
6699  vinfos[4].maxsolutions = _nj4;
6700  vinfos[5].jointtype = 1;
6701  vinfos[5].foffset = j5;
6702  vinfos[5].indices[0] = _ij5[0];
6703  vinfos[5].indices[1] = _ij5[1];
6704  vinfos[5].maxsolutions = _nj5;
6705  vinfos[6].jointtype = 1;
6706  vinfos[6].foffset = j6;
6707  vinfos[6].indices[0] = _ij6[0];
6708  vinfos[6].indices[1] = _ij6[1];
6709  vinfos[6].maxsolutions = _nj6;
6710  std::vector<int> vfree(0);
6711  solutions.AddSolution(vinfos, vfree);
6712  }
6713  }
6714  }
6715  }
6716  } while (0);
6717  if (bgotonextstatement)
6718  {
6719  bool bgotonextstatement = true;
6720  do
6721  {
6722  evalcond[0] = ((-3.14159265358979) +
6723  (IKfmod(((3.14159265358979) +
6724  (IKabs(((-1.5707963267949) + j0)))),
6725  6.28318530717959)));
6726  evalcond[1] = new_r02;
6727  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6728  IKabs(evalcond[1]) < 0.0000050000000000)
6729  {
6730  bgotonextstatement = false;
6731  {
6732  IkReal j2array[1], cj2array[1], sj2array[1];
6733  bool j2valid[1] = { false };
6734  _nj2 = 1;
6735  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
6736  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
6737  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
6738  IKFAST_SINCOS_THRESH)
6739  continue;
6740  j2array[0] = IKatan2(new_r00, new_r01);
6741  sj2array[0] = IKsin(j2array[0]);
6742  cj2array[0] = IKcos(j2array[0]);
6743  if (j2array[0] > IKPI)
6744  {
6745  j2array[0] -= IK2PI;
6746  }
6747  else if (j2array[0] < -IKPI)
6748  {
6749  j2array[0] += IK2PI;
6750  }
6751  j2valid[0] = true;
6752  for (int ij2 = 0; ij2 < 1; ++ij2)
6753  {
6754  if (!j2valid[ij2])
6755  {
6756  continue;
6757  }
6758  _ij2[0] = ij2;
6759  _ij2[1] = -1;
6760  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6761  {
6762  if (j2valid[iij2] &&
6763  IKabs(cj2array[ij2] - cj2array[iij2]) <
6764  IKFAST_SOLUTION_THRESH &&
6765  IKabs(sj2array[ij2] - sj2array[iij2]) <
6766  IKFAST_SOLUTION_THRESH)
6767  {
6768  j2valid[iij2] = false;
6769  _ij2[1] = iij2;
6770  break;
6771  }
6772  }
6773  j2 = j2array[ij2];
6774  cj2 = cj2array[ij2];
6775  sj2 = sj2array[ij2];
6776  {
6777  IkReal evalcond[8];
6778  IkReal x256 = IKsin(j2);
6779  IkReal x257 = IKcos(j2);
6780  IkReal x258 = ((1.0) * sj1);
6781  IkReal x259 = ((1.0) * x256);
6782  evalcond[0] = (((sj1 * x256)) + new_r21);
6783  evalcond[1] = (x256 + (((-1.0) * new_r00)));
6784  evalcond[2] = (x257 + (((-1.0) * new_r01)));
6785  evalcond[3] = (((cj1 * x257)) + new_r10);
6786  evalcond[4] = ((((-1.0) * x257 * x258)) + new_r20);
6787  evalcond[5] = ((((-1.0) * cj1 * x259)) + new_r11);
6788  evalcond[6] = (((cj1 * new_r10)) + x257 +
6789  (((-1.0) * new_r20 * x258)));
6790  evalcond[7] = ((((-1.0) * new_r21 * x258)) +
6791  ((cj1 * new_r11)) + (((-1.0) * x259)));
6792  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6793  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6794  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6795  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6796  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6797  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6798  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6799  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6800  {
6801  continue;
6802  }
6803  }
6804 
6805  {
6806  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6807  vinfos[0].jointtype = 1;
6808  vinfos[0].foffset = j0;
6809  vinfos[0].indices[0] = _ij0[0];
6810  vinfos[0].indices[1] = _ij0[1];
6811  vinfos[0].maxsolutions = _nj0;
6812  vinfos[1].jointtype = 1;
6813  vinfos[1].foffset = j1;
6814  vinfos[1].indices[0] = _ij1[0];
6815  vinfos[1].indices[1] = _ij1[1];
6816  vinfos[1].maxsolutions = _nj1;
6817  vinfos[2].jointtype = 1;
6818  vinfos[2].foffset = j2;
6819  vinfos[2].indices[0] = _ij2[0];
6820  vinfos[2].indices[1] = _ij2[1];
6821  vinfos[2].maxsolutions = _nj2;
6822  vinfos[3].jointtype = 1;
6823  vinfos[3].foffset = j3;
6824  vinfos[3].indices[0] = _ij3[0];
6825  vinfos[3].indices[1] = _ij3[1];
6826  vinfos[3].maxsolutions = _nj3;
6827  vinfos[4].jointtype = 1;
6828  vinfos[4].foffset = j4;
6829  vinfos[4].indices[0] = _ij4[0];
6830  vinfos[4].indices[1] = _ij4[1];
6831  vinfos[4].maxsolutions = _nj4;
6832  vinfos[5].jointtype = 1;
6833  vinfos[5].foffset = j5;
6834  vinfos[5].indices[0] = _ij5[0];
6835  vinfos[5].indices[1] = _ij5[1];
6836  vinfos[5].maxsolutions = _nj5;
6837  vinfos[6].jointtype = 1;
6838  vinfos[6].foffset = j6;
6839  vinfos[6].indices[0] = _ij6[0];
6840  vinfos[6].indices[1] = _ij6[1];
6841  vinfos[6].maxsolutions = _nj6;
6842  std::vector<int> vfree(0);
6843  solutions.AddSolution(vinfos, vfree);
6844  }
6845  }
6846  }
6847  }
6848  } while (0);
6849  if (bgotonextstatement)
6850  {
6851  bool bgotonextstatement = true;
6852  do
6853  {
6854  evalcond[0] = ((-3.14159265358979) +
6855  (IKfmod(((3.14159265358979) +
6856  (IKabs(((1.5707963267949) + j0)))),
6857  6.28318530717959)));
6858  evalcond[1] = new_r02;
6859  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6860  IKabs(evalcond[1]) < 0.0000050000000000)
6861  {
6862  bgotonextstatement = false;
6863  {
6864  IkReal j2array[1], cj2array[1], sj2array[1];
6865  bool j2valid[1] = { false };
6866  _nj2 = 1;
6867  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
6868  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
6869  IKabs(IKsqr(((-1.0) * new_r00)) +
6870  IKsqr(((-1.0) * new_r01)) - 1) <=
6871  IKFAST_SINCOS_THRESH)
6872  continue;
6873  j2array[0] =
6874  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
6875  sj2array[0] = IKsin(j2array[0]);
6876  cj2array[0] = IKcos(j2array[0]);
6877  if (j2array[0] > IKPI)
6878  {
6879  j2array[0] -= IK2PI;
6880  }
6881  else if (j2array[0] < -IKPI)
6882  {
6883  j2array[0] += IK2PI;
6884  }
6885  j2valid[0] = true;
6886  for (int ij2 = 0; ij2 < 1; ++ij2)
6887  {
6888  if (!j2valid[ij2])
6889  {
6890  continue;
6891  }
6892  _ij2[0] = ij2;
6893  _ij2[1] = -1;
6894  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6895  {
6896  if (j2valid[iij2] &&
6897  IKabs(cj2array[ij2] - cj2array[iij2]) <
6898  IKFAST_SOLUTION_THRESH &&
6899  IKabs(sj2array[ij2] - sj2array[iij2]) <
6900  IKFAST_SOLUTION_THRESH)
6901  {
6902  j2valid[iij2] = false;
6903  _ij2[1] = iij2;
6904  break;
6905  }
6906  }
6907  j2 = j2array[ij2];
6908  cj2 = cj2array[ij2];
6909  sj2 = sj2array[ij2];
6910  {
6911  IkReal evalcond[8];
6912  IkReal x260 = IKsin(j2);
6913  IkReal x261 = IKcos(j2);
6914  IkReal x262 = ((1.0) * new_r11);
6915  IkReal x263 = ((1.0) * sj1);
6916  IkReal x264 = ((1.0) * new_r10);
6917  IkReal x265 = ((1.0) * x260);
6918  evalcond[0] = (x260 + new_r00);
6919  evalcond[1] = (x261 + new_r01);
6920  evalcond[2] = (new_r21 + ((sj1 * x260)));
6921  evalcond[3] = ((((-1.0) * x261 * x263)) + new_r20);
6922  evalcond[4] = (((cj1 * x261)) + (((-1.0) * x264)));
6923  evalcond[5] =
6924  ((((-1.0) * cj1 * x265)) + (((-1.0) * x262)));
6925  evalcond[6] = ((((-1.0) * cj1 * x264)) + x261 +
6926  (((-1.0) * new_r20 * x263)));
6927  evalcond[7] =
6928  ((((-1.0) * cj1 * x262)) +
6929  (((-1.0) * new_r21 * x263)) + (((-1.0) * x265)));
6930  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6931  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6932  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6933  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6934  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6935  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6936  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6937  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6938  {
6939  continue;
6940  }
6941  }
6942 
6943  {
6944  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6945  vinfos[0].jointtype = 1;
6946  vinfos[0].foffset = j0;
6947  vinfos[0].indices[0] = _ij0[0];
6948  vinfos[0].indices[1] = _ij0[1];
6949  vinfos[0].maxsolutions = _nj0;
6950  vinfos[1].jointtype = 1;
6951  vinfos[1].foffset = j1;
6952  vinfos[1].indices[0] = _ij1[0];
6953  vinfos[1].indices[1] = _ij1[1];
6954  vinfos[1].maxsolutions = _nj1;
6955  vinfos[2].jointtype = 1;
6956  vinfos[2].foffset = j2;
6957  vinfos[2].indices[0] = _ij2[0];
6958  vinfos[2].indices[1] = _ij2[1];
6959  vinfos[2].maxsolutions = _nj2;
6960  vinfos[3].jointtype = 1;
6961  vinfos[3].foffset = j3;
6962  vinfos[3].indices[0] = _ij3[0];
6963  vinfos[3].indices[1] = _ij3[1];
6964  vinfos[3].maxsolutions = _nj3;
6965  vinfos[4].jointtype = 1;
6966  vinfos[4].foffset = j4;
6967  vinfos[4].indices[0] = _ij4[0];
6968  vinfos[4].indices[1] = _ij4[1];
6969  vinfos[4].maxsolutions = _nj4;
6970  vinfos[5].jointtype = 1;
6971  vinfos[5].foffset = j5;
6972  vinfos[5].indices[0] = _ij5[0];
6973  vinfos[5].indices[1] = _ij5[1];
6974  vinfos[5].maxsolutions = _nj5;
6975  vinfos[6].jointtype = 1;
6976  vinfos[6].foffset = j6;
6977  vinfos[6].indices[0] = _ij6[0];
6978  vinfos[6].indices[1] = _ij6[1];
6979  vinfos[6].maxsolutions = _nj6;
6980  std::vector<int> vfree(0);
6981  solutions.AddSolution(vinfos, vfree);
6982  }
6983  }
6984  }
6985  }
6986  } while (0);
6987  if (bgotonextstatement)
6988  {
6989  bool bgotonextstatement = true;
6990  do
6991  {
6992  evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
6993  if (IKabs(evalcond[0]) < 0.0000050000000000)
6994  {
6995  bgotonextstatement = false;
6996  {
6997  IkReal j2eval[1];
6998  new_r21 = 0;
6999  new_r20 = 0;
7000  new_r02 = 0;
7001  new_r12 = 0;
7002  j2eval[0] = IKabs(new_r22);
7003  if (IKabs(j2eval[0]) < 0.0000000100000000)
7004  {
7005  continue; // no branches [j2]
7006  }
7007  else
7008  {
7009  IkReal op[2 + 1], zeror[2];
7010  int numroots;
7011  op[0] = ((-1.0) * new_r22);
7012  op[1] = 0;
7013  op[2] = new_r22;
7014  polyroots2(op, zeror, numroots);
7015  IkReal j2array[2], cj2array[2], sj2array[2],
7016  tempj2array[1];
7017  int numsolutions = 0;
7018  for (int ij2 = 0; ij2 < numroots; ++ij2)
7019  {
7020  IkReal htj2 = zeror[ij2];
7021  tempj2array[0] = ((2.0) * (atan(htj2)));
7022  for (int kj2 = 0; kj2 < 1; ++kj2)
7023  {
7024  j2array[numsolutions] = tempj2array[kj2];
7025  if (j2array[numsolutions] > IKPI)
7026  {
7027  j2array[numsolutions] -= IK2PI;
7028  }
7029  else if (j2array[numsolutions] < -IKPI)
7030  {
7031  j2array[numsolutions] += IK2PI;
7032  }
7033  sj2array[numsolutions] =
7034  IKsin(j2array[numsolutions]);
7035  cj2array[numsolutions] =
7036  IKcos(j2array[numsolutions]);
7037  numsolutions++;
7038  }
7039  }
7040  bool j2valid[2] = { true, true };
7041  _nj2 = 2;
7042  for (int ij2 = 0; ij2 < numsolutions; ++ij2)
7043  {
7044  if (!j2valid[ij2])
7045  {
7046  continue;
7047  }
7048  j2 = j2array[ij2];
7049  cj2 = cj2array[ij2];
7050  sj2 = sj2array[ij2];
7051  htj2 = IKtan(j2 / 2);
7052 
7053  _ij2[0] = ij2;
7054  _ij2[1] = -1;
7055  for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
7056  {
7057  if (j2valid[iij2] &&
7058  IKabs(cj2array[ij2] - cj2array[iij2]) <
7059  IKFAST_SOLUTION_THRESH &&
7060  IKabs(sj2array[ij2] - sj2array[iij2]) <
7061  IKFAST_SOLUTION_THRESH)
7062  {
7063  j2valid[iij2] = false;
7064  _ij2[1] = iij2;
7065  break;
7066  }
7067  }
7068  {
7069  std::vector<IkSingleDOFSolutionBase<IkReal> >
7070  vinfos(7);
7071  vinfos[0].jointtype = 1;
7072  vinfos[0].foffset = j0;
7073  vinfos[0].indices[0] = _ij0[0];
7074  vinfos[0].indices[1] = _ij0[1];
7075  vinfos[0].maxsolutions = _nj0;
7076  vinfos[1].jointtype = 1;
7077  vinfos[1].foffset = j1;
7078  vinfos[1].indices[0] = _ij1[0];
7079  vinfos[1].indices[1] = _ij1[1];
7080  vinfos[1].maxsolutions = _nj1;
7081  vinfos[2].jointtype = 1;
7082  vinfos[2].foffset = j2;
7083  vinfos[2].indices[0] = _ij2[0];
7084  vinfos[2].indices[1] = _ij2[1];
7085  vinfos[2].maxsolutions = _nj2;
7086  vinfos[3].jointtype = 1;
7087  vinfos[3].foffset = j3;
7088  vinfos[3].indices[0] = _ij3[0];
7089  vinfos[3].indices[1] = _ij3[1];
7090  vinfos[3].maxsolutions = _nj3;
7091  vinfos[4].jointtype = 1;
7092  vinfos[4].foffset = j4;
7093  vinfos[4].indices[0] = _ij4[0];
7094  vinfos[4].indices[1] = _ij4[1];
7095  vinfos[4].maxsolutions = _nj4;
7096  vinfos[5].jointtype = 1;
7097  vinfos[5].foffset = j5;
7098  vinfos[5].indices[0] = _ij5[0];
7099  vinfos[5].indices[1] = _ij5[1];
7100  vinfos[5].maxsolutions = _nj5;
7101  vinfos[6].jointtype = 1;
7102  vinfos[6].foffset = j6;
7103  vinfos[6].indices[0] = _ij6[0];
7104  vinfos[6].indices[1] = _ij6[1];
7105  vinfos[6].maxsolutions = _nj6;
7106  std::vector<int> vfree(0);
7107  solutions.AddSolution(vinfos, vfree);
7108  }
7109  }
7110  }
7111  }
7112  }
7113  } while (0);
7114  if (bgotonextstatement)
7115  {
7116  bool bgotonextstatement = true;
7117  do
7118  {
7119  if (1)
7120  {
7121  bgotonextstatement = false;
7122  continue; // branch miss [j2]
7123  }
7124  } while (0);
7125  if (bgotonextstatement)
7126  {
7127  }
7128  }
7129  }
7130  }
7131  }
7132  }
7133  }
7134  }
7135  }
7136  }
7137  }
7138  }
7139  else
7140  {
7141  {
7142  IkReal j2array[1], cj2array[1], sj2array[1];
7143  bool j2valid[1] = { false };
7144  _nj2 = 1;
7145  CheckValue<IkReal> x267 = IKPowWithIntegerCheck(sj1, -1);
7146  if (!x267.valid)
7147  {
7148  continue;
7149  }
7150  IkReal x266 = x267.value;
7151  CheckValue<IkReal> x268 = IKPowWithIntegerCheck(cj1, -1);
7152  if (!x268.valid)
7153  {
7154  continue;
7155  }
7156  CheckValue<IkReal> x269 = IKPowWithIntegerCheck(sj0, -1);
7157  if (!x269.valid)
7158  {
7159  continue;
7160  }
7161  if (IKabs(((-1.0) * new_r21 * x266)) < IKFAST_ATAN2_MAGTHRESH &&
7162  IKabs((x266 * (x268.value) * (x269.value) *
7163  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
7164  IKFAST_ATAN2_MAGTHRESH &&
7165  IKabs(IKsqr(((-1.0) * new_r21 * x266)) +
7166  IKsqr((x266 * (x268.value) * (x269.value) *
7167  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
7168  1) <= IKFAST_SINCOS_THRESH)
7169  continue;
7170  j2array[0] = IKatan2(((-1.0) * new_r21 * x266),
7171  (x266 * (x268.value) * (x269.value) *
7172  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
7173  sj2array[0] = IKsin(j2array[0]);
7174  cj2array[0] = IKcos(j2array[0]);
7175  if (j2array[0] > IKPI)
7176  {
7177  j2array[0] -= IK2PI;
7178  }
7179  else if (j2array[0] < -IKPI)
7180  {
7181  j2array[0] += IK2PI;
7182  }
7183  j2valid[0] = true;
7184  for (int ij2 = 0; ij2 < 1; ++ij2)
7185  {
7186  if (!j2valid[ij2])
7187  {
7188  continue;
7189  }
7190  _ij2[0] = ij2;
7191  _ij2[1] = -1;
7192  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7193  {
7194  if (j2valid[iij2] &&
7195  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7196  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7197  {
7198  j2valid[iij2] = false;
7199  _ij2[1] = iij2;
7200  break;
7201  }
7202  }
7203  j2 = j2array[ij2];
7204  cj2 = cj2array[ij2];
7205  sj2 = sj2array[ij2];
7206  {
7207  IkReal evalcond[12];
7208  IkReal x270 = IKsin(j2);
7209  IkReal x271 = IKcos(j2);
7210  IkReal x272 = ((1.0) * sj0);
7211  IkReal x273 = ((1.0) * sj1);
7212  IkReal x274 = (cj0 * new_r00);
7213  IkReal x275 = (cj1 * sj0);
7214  IkReal x276 = (cj0 * new_r01);
7215  IkReal x277 = (cj1 * x271);
7216  IkReal x278 = (cj1 * x270);
7217  evalcond[0] = (new_r21 + ((sj1 * x270)));
7218  evalcond[1] = ((((-1.0) * x271 * x273)) + new_r20);
7219  evalcond[2] = ((((-1.0) * new_r00 * x272)) + x270 + ((cj0 * new_r10)));
7220  evalcond[3] = ((((-1.0) * new_r01 * x272)) + x271 + ((cj0 * new_r11)));
7221  evalcond[4] = (((new_r10 * sj0)) + x277 + x274);
7222  evalcond[5] = (((x271 * x275)) + new_r10 + ((cj0 * x270)));
7223  evalcond[6] = ((((-1.0) * x278)) + ((new_r11 * sj0)) + x276);
7224  evalcond[7] = ((((-1.0) * x270 * x272)) + new_r00 + ((cj0 * x277)));
7225  evalcond[8] = ((((-1.0) * x272 * x278)) + new_r11 + ((cj0 * x271)));
7226  evalcond[9] =
7227  ((((-1.0) * cj0 * x278)) + (((-1.0) * x271 * x272)) + new_r01);
7228  evalcond[10] = (((cj1 * x274)) + x271 + (((-1.0) * new_r20 * x273)) +
7229  ((new_r10 * x275)));
7230  evalcond[11] = ((((-1.0) * x270)) + ((cj1 * x276)) +
7231  (((-1.0) * new_r21 * x273)) + ((new_r11 * x275)));
7232  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7233  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7234  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7235  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7236  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7237  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7238  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7239  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7240  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7241  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7242  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7243  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7244  {
7245  continue;
7246  }
7247  }
7248 
7249  {
7250  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7251  vinfos[0].jointtype = 1;
7252  vinfos[0].foffset = j0;
7253  vinfos[0].indices[0] = _ij0[0];
7254  vinfos[0].indices[1] = _ij0[1];
7255  vinfos[0].maxsolutions = _nj0;
7256  vinfos[1].jointtype = 1;
7257  vinfos[1].foffset = j1;
7258  vinfos[1].indices[0] = _ij1[0];
7259  vinfos[1].indices[1] = _ij1[1];
7260  vinfos[1].maxsolutions = _nj1;
7261  vinfos[2].jointtype = 1;
7262  vinfos[2].foffset = j2;
7263  vinfos[2].indices[0] = _ij2[0];
7264  vinfos[2].indices[1] = _ij2[1];
7265  vinfos[2].maxsolutions = _nj2;
7266  vinfos[3].jointtype = 1;
7267  vinfos[3].foffset = j3;
7268  vinfos[3].indices[0] = _ij3[0];
7269  vinfos[3].indices[1] = _ij3[1];
7270  vinfos[3].maxsolutions = _nj3;
7271  vinfos[4].jointtype = 1;
7272  vinfos[4].foffset = j4;
7273  vinfos[4].indices[0] = _ij4[0];
7274  vinfos[4].indices[1] = _ij4[1];
7275  vinfos[4].maxsolutions = _nj4;
7276  vinfos[5].jointtype = 1;
7277  vinfos[5].foffset = j5;
7278  vinfos[5].indices[0] = _ij5[0];
7279  vinfos[5].indices[1] = _ij5[1];
7280  vinfos[5].maxsolutions = _nj5;
7281  vinfos[6].jointtype = 1;
7282  vinfos[6].foffset = j6;
7283  vinfos[6].indices[0] = _ij6[0];
7284  vinfos[6].indices[1] = _ij6[1];
7285  vinfos[6].maxsolutions = _nj6;
7286  std::vector<int> vfree(0);
7287  solutions.AddSolution(vinfos, vfree);
7288  }
7289  }
7290  }
7291  }
7292  }
7293  }
7294  else
7295  {
7296  {
7297  IkReal j2array[1], cj2array[1], sj2array[1];
7298  bool j2valid[1] = { false };
7299  _nj2 = 1;
7300  CheckValue<IkReal> x280 = IKPowWithIntegerCheck(sj1, -1);
7301  if (!x280.valid)
7302  {
7303  continue;
7304  }
7305  IkReal x279 = x280.value;
7306  CheckValue<IkReal> x281 = IKPowWithIntegerCheck(cj0, -1);
7307  if (!x281.valid)
7308  {
7309  continue;
7310  }
7311  if (IKabs(((-1.0) * new_r21 * x279)) < IKFAST_ATAN2_MAGTHRESH &&
7312  IKabs((x279 * (x281.value) *
7313  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
7314  IKFAST_ATAN2_MAGTHRESH &&
7315  IKabs(
7316  IKsqr(((-1.0) * new_r21 * x279)) +
7317  IKsqr((x279 * (x281.value) *
7318  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
7319  1) <= IKFAST_SINCOS_THRESH)
7320  continue;
7321  j2array[0] =
7322  IKatan2(((-1.0) * new_r21 * x279),
7323  (x279 * (x281.value) *
7324  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
7325  sj2array[0] = IKsin(j2array[0]);
7326  cj2array[0] = IKcos(j2array[0]);
7327  if (j2array[0] > IKPI)
7328  {
7329  j2array[0] -= IK2PI;
7330  }
7331  else if (j2array[0] < -IKPI)
7332  {
7333  j2array[0] += IK2PI;
7334  }
7335  j2valid[0] = true;
7336  for (int ij2 = 0; ij2 < 1; ++ij2)
7337  {
7338  if (!j2valid[ij2])
7339  {
7340  continue;
7341  }
7342  _ij2[0] = ij2;
7343  _ij2[1] = -1;
7344  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7345  {
7346  if (j2valid[iij2] &&
7347  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7348  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7349  {
7350  j2valid[iij2] = false;
7351  _ij2[1] = iij2;
7352  break;
7353  }
7354  }
7355  j2 = j2array[ij2];
7356  cj2 = cj2array[ij2];
7357  sj2 = sj2array[ij2];
7358  {
7359  IkReal evalcond[12];
7360  IkReal x282 = IKsin(j2);
7361  IkReal x283 = IKcos(j2);
7362  IkReal x284 = ((1.0) * sj0);
7363  IkReal x285 = ((1.0) * sj1);
7364  IkReal x286 = (cj0 * new_r00);
7365  IkReal x287 = (cj1 * sj0);
7366  IkReal x288 = (cj0 * new_r01);
7367  IkReal x289 = (cj1 * x283);
7368  IkReal x290 = (cj1 * x282);
7369  evalcond[0] = (new_r21 + ((sj1 * x282)));
7370  evalcond[1] = (new_r20 + (((-1.0) * x283 * x285)));
7371  evalcond[2] = ((((-1.0) * new_r00 * x284)) + x282 + ((cj0 * new_r10)));
7372  evalcond[3] = (x283 + (((-1.0) * new_r01 * x284)) + ((cj0 * new_r11)));
7373  evalcond[4] = (((new_r10 * sj0)) + x289 + x286);
7374  evalcond[5] = (((cj0 * x282)) + new_r10 + ((x283 * x287)));
7375  evalcond[6] = ((((-1.0) * x290)) + ((new_r11 * sj0)) + x288);
7376  evalcond[7] = (((cj0 * x289)) + (((-1.0) * x282 * x284)) + new_r00);
7377  evalcond[8] = ((((-1.0) * x284 * x290)) + ((cj0 * x283)) + new_r11);
7378  evalcond[9] = ((((-1.0) * cj0 * x290)) + new_r01 + (((-1.0) * x283 * x284)));
7379  evalcond[10] =
7380  (x283 + (((-1.0) * new_r20 * x285)) + ((cj1 * x286)) + ((new_r10 * x287)));
7381  evalcond[11] = ((((-1.0) * x282)) + (((-1.0) * new_r21 * x285)) +
7382  ((new_r11 * x287)) + ((cj1 * x288)));
7383  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7384  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7385  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7386  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7387  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7388  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7389  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7390  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7391  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7392  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7393  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7394  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7395  {
7396  continue;
7397  }
7398  }
7399 
7400  {
7401  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7402  vinfos[0].jointtype = 1;
7403  vinfos[0].foffset = j0;
7404  vinfos[0].indices[0] = _ij0[0];
7405  vinfos[0].indices[1] = _ij0[1];
7406  vinfos[0].maxsolutions = _nj0;
7407  vinfos[1].jointtype = 1;
7408  vinfos[1].foffset = j1;
7409  vinfos[1].indices[0] = _ij1[0];
7410  vinfos[1].indices[1] = _ij1[1];
7411  vinfos[1].maxsolutions = _nj1;
7412  vinfos[2].jointtype = 1;
7413  vinfos[2].foffset = j2;
7414  vinfos[2].indices[0] = _ij2[0];
7415  vinfos[2].indices[1] = _ij2[1];
7416  vinfos[2].maxsolutions = _nj2;
7417  vinfos[3].jointtype = 1;
7418  vinfos[3].foffset = j3;
7419  vinfos[3].indices[0] = _ij3[0];
7420  vinfos[3].indices[1] = _ij3[1];
7421  vinfos[3].maxsolutions = _nj3;
7422  vinfos[4].jointtype = 1;
7423  vinfos[4].foffset = j4;
7424  vinfos[4].indices[0] = _ij4[0];
7425  vinfos[4].indices[1] = _ij4[1];
7426  vinfos[4].maxsolutions = _nj4;
7427  vinfos[5].jointtype = 1;
7428  vinfos[5].foffset = j5;
7429  vinfos[5].indices[0] = _ij5[0];
7430  vinfos[5].indices[1] = _ij5[1];
7431  vinfos[5].maxsolutions = _nj5;
7432  vinfos[6].jointtype = 1;
7433  vinfos[6].foffset = j6;
7434  vinfos[6].indices[0] = _ij6[0];
7435  vinfos[6].indices[1] = _ij6[1];
7436  vinfos[6].maxsolutions = _nj6;
7437  std::vector<int> vfree(0);
7438  solutions.AddSolution(vinfos, vfree);
7439  }
7440  }
7441  }
7442  }
7443  }
7444  }
7445  else
7446  {
7447  {
7448  IkReal j2array[1], cj2array[1], sj2array[1];
7449  bool j2valid[1] = { false };
7450  _nj2 = 1;
7451  CheckValue<IkReal> x291 = IKatan2WithCheck(
7452  IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7453  if (!x291.valid)
7454  {
7455  continue;
7456  }
7457  CheckValue<IkReal> x292 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7458  if (!x292.valid)
7459  {
7460  continue;
7461  }
7462  j2array[0] = ((-1.5707963267949) + (x291.value) + (((1.5707963267949) * (x292.value))));
7463  sj2array[0] = IKsin(j2array[0]);
7464  cj2array[0] = IKcos(j2array[0]);
7465  if (j2array[0] > IKPI)
7466  {
7467  j2array[0] -= IK2PI;
7468  }
7469  else if (j2array[0] < -IKPI)
7470  {
7471  j2array[0] += IK2PI;
7472  }
7473  j2valid[0] = true;
7474  for (int ij2 = 0; ij2 < 1; ++ij2)
7475  {
7476  if (!j2valid[ij2])
7477  {
7478  continue;
7479  }
7480  _ij2[0] = ij2;
7481  _ij2[1] = -1;
7482  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7483  {
7484  if (j2valid[iij2] &&
7485  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7486  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7487  {
7488  j2valid[iij2] = false;
7489  _ij2[1] = iij2;
7490  break;
7491  }
7492  }
7493  j2 = j2array[ij2];
7494  cj2 = cj2array[ij2];
7495  sj2 = sj2array[ij2];
7496  {
7497  IkReal evalcond[12];
7498  IkReal x293 = IKsin(j2);
7499  IkReal x294 = IKcos(j2);
7500  IkReal x295 = ((1.0) * sj0);
7501  IkReal x296 = ((1.0) * sj1);
7502  IkReal x297 = (cj0 * new_r00);
7503  IkReal x298 = (cj1 * sj0);
7504  IkReal x299 = (cj0 * new_r01);
7505  IkReal x300 = (cj1 * x294);
7506  IkReal x301 = (cj1 * x293);
7507  evalcond[0] = (((sj1 * x293)) + new_r21);
7508  evalcond[1] = (new_r20 + (((-1.0) * x294 * x296)));
7509  evalcond[2] = (x293 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x295)));
7510  evalcond[3] = ((((-1.0) * new_r01 * x295)) + x294 + ((cj0 * new_r11)));
7511  evalcond[4] = (((new_r10 * sj0)) + x300 + x297);
7512  evalcond[5] = (((x294 * x298)) + new_r10 + ((cj0 * x293)));
7513  evalcond[6] = (((new_r11 * sj0)) + (((-1.0) * x301)) + x299);
7514  evalcond[7] = ((((-1.0) * x293 * x295)) + ((cj0 * x300)) + new_r00);
7515  evalcond[8] = ((((-1.0) * x295 * x301)) + new_r11 + ((cj0 * x294)));
7516  evalcond[9] = ((((-1.0) * cj0 * x301)) + new_r01 + (((-1.0) * x294 * x295)));
7517  evalcond[10] =
7518  ((((-1.0) * new_r20 * x296)) + ((cj1 * x297)) + ((new_r10 * x298)) + x294);
7519  evalcond[11] = ((((-1.0) * x293)) + (((-1.0) * new_r21 * x296)) +
7520  ((new_r11 * x298)) + ((cj1 * x299)));
7521  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7522  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7523  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7524  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7525  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7526  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7527  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7528  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7529  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7530  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7531  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7532  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7533  {
7534  continue;
7535  }
7536  }
7537 
7538  {
7539  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7540  vinfos[0].jointtype = 1;
7541  vinfos[0].foffset = j0;
7542  vinfos[0].indices[0] = _ij0[0];
7543  vinfos[0].indices[1] = _ij0[1];
7544  vinfos[0].maxsolutions = _nj0;
7545  vinfos[1].jointtype = 1;
7546  vinfos[1].foffset = j1;
7547  vinfos[1].indices[0] = _ij1[0];
7548  vinfos[1].indices[1] = _ij1[1];
7549  vinfos[1].maxsolutions = _nj1;
7550  vinfos[2].jointtype = 1;
7551  vinfos[2].foffset = j2;
7552  vinfos[2].indices[0] = _ij2[0];
7553  vinfos[2].indices[1] = _ij2[1];
7554  vinfos[2].maxsolutions = _nj2;
7555  vinfos[3].jointtype = 1;
7556  vinfos[3].foffset = j3;
7557  vinfos[3].indices[0] = _ij3[0];
7558  vinfos[3].indices[1] = _ij3[1];
7559  vinfos[3].maxsolutions = _nj3;
7560  vinfos[4].jointtype = 1;
7561  vinfos[4].foffset = j4;
7562  vinfos[4].indices[0] = _ij4[0];
7563  vinfos[4].indices[1] = _ij4[1];
7564  vinfos[4].maxsolutions = _nj4;
7565  vinfos[5].jointtype = 1;
7566  vinfos[5].foffset = j5;
7567  vinfos[5].indices[0] = _ij5[0];
7568  vinfos[5].indices[1] = _ij5[1];
7569  vinfos[5].maxsolutions = _nj5;
7570  vinfos[6].jointtype = 1;
7571  vinfos[6].foffset = j6;
7572  vinfos[6].indices[0] = _ij6[0];
7573  vinfos[6].indices[1] = _ij6[1];
7574  vinfos[6].maxsolutions = _nj6;
7575  std::vector<int> vfree(0);
7576  solutions.AddSolution(vinfos, vfree);
7577  }
7578  }
7579  }
7580  }
7581  }
7582  }
7583  }
7584  }
7585  }
7586  }
7587  else
7588  {
7589  {
7590  IkReal j2array[1], cj2array[1], sj2array[1];
7591  bool j2valid[1] = { false };
7592  _nj2 = 1;
7593  CheckValue<IkReal> x302 =
7594  IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7595  if (!x302.valid)
7596  {
7597  continue;
7598  }
7599  CheckValue<IkReal> x303 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7600  if (!x303.valid)
7601  {
7602  continue;
7603  }
7604  j2array[0] = ((-1.5707963267949) + (x302.value) + (((1.5707963267949) * (x303.value))));
7605  sj2array[0] = IKsin(j2array[0]);
7606  cj2array[0] = IKcos(j2array[0]);
7607  if (j2array[0] > IKPI)
7608  {
7609  j2array[0] -= IK2PI;
7610  }
7611  else if (j2array[0] < -IKPI)
7612  {
7613  j2array[0] += IK2PI;
7614  }
7615  j2valid[0] = true;
7616  for (int ij2 = 0; ij2 < 1; ++ij2)
7617  {
7618  if (!j2valid[ij2])
7619  {
7620  continue;
7621  }
7622  _ij2[0] = ij2;
7623  _ij2[1] = -1;
7624  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7625  {
7626  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7627  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7628  {
7629  j2valid[iij2] = false;
7630  _ij2[1] = iij2;
7631  break;
7632  }
7633  }
7634  j2 = j2array[ij2];
7635  cj2 = cj2array[ij2];
7636  sj2 = sj2array[ij2];
7637  {
7638  IkReal evalcond[2];
7639  evalcond[0] = (((sj1 * (IKsin(j2)))) + new_r21);
7640  evalcond[1] = ((((-1.0) * sj1 * (IKcos(j2)))) + new_r20);
7641  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
7642  {
7643  continue;
7644  }
7645  }
7646 
7647  {
7648  IkReal j0eval[3];
7649  j0eval[0] = sj1;
7650  j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
7651  j0eval[2] = IKsign(sj1);
7652  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
7653  IKabs(j0eval[2]) < 0.0000010000000000)
7654  {
7655  {
7656  IkReal j0eval[2];
7657  j0eval[0] = new_r00;
7658  j0eval[1] = sj1;
7659  if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
7660  {
7661  {
7662  IkReal evalcond[5];
7663  bool bgotonextstatement = true;
7664  do
7665  {
7666  evalcond[0] = ((-3.14159265358979) +
7667  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
7668  evalcond[1] = new_r21;
7669  evalcond[2] = new_r02;
7670  evalcond[3] = new_r12;
7671  evalcond[4] = new_r20;
7672  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
7673  IKabs(evalcond[1]) < 0.0000050000000000 &&
7674  IKabs(evalcond[2]) < 0.0000050000000000 &&
7675  IKabs(evalcond[3]) < 0.0000050000000000 &&
7676  IKabs(evalcond[4]) < 0.0000050000000000)
7677  {
7678  bgotonextstatement = false;
7679  {
7680  IkReal j0eval[3];
7681  sj1 = 0;
7682  cj1 = 1.0;
7683  j1 = 0;
7684  IkReal x304 = ((1.0) * cj2);
7685  IkReal x305 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7686  j0eval[0] = x305;
7687  j0eval[1] = ((IKabs((((new_r00 * sj2)) + (((-1.0) * new_r10 * x304))))) +
7688  (IKabs(((((-1.0) * new_r10 * sj2)) + (((-1.0) * new_r00 * x304))))));
7689  j0eval[2] = IKsign(x305);
7690  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7691  IKabs(j0eval[1]) < 0.0000010000000000 ||
7692  IKabs(j0eval[2]) < 0.0000010000000000)
7693  {
7694  {
7695  IkReal j0eval[3];
7696  sj1 = 0;
7697  cj1 = 1.0;
7698  j1 = 0;
7699  IkReal x306 = ((1.0) * cj2);
7700  IkReal x307 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7701  j0eval[0] = x307;
7702  j0eval[1] =
7703  ((IKabs(((((-1.0) * new_r01 * x306)) + (((-1.0) * new_r10 * x306))))) +
7704  (IKabs((((cj2 * new_r00)) + (((-1.0) * new_r11 * x306))))));
7705  j0eval[2] = IKsign(x307);
7706  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7707  IKabs(j0eval[1]) < 0.0000010000000000 ||
7708  IKabs(j0eval[2]) < 0.0000010000000000)
7709  {
7710  {
7711  IkReal j0eval[3];
7712  sj1 = 0;
7713  cj1 = 1.0;
7714  j1 = 0;
7715  IkReal x308 = ((1.0) * new_r10);
7716  IkReal x309 = ((((-1.0) * sj2 * x308)) + ((cj2 * new_r00)));
7717  j0eval[0] = x309;
7718  j0eval[1] = IKsign(x309);
7719  j0eval[2] = ((IKabs(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10)))) +
7720  (IKabs((((cj2 * sj2)) + (((-1.0) * new_r00 * x308))))));
7721  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7722  IKabs(j0eval[1]) < 0.0000010000000000 ||
7723  IKabs(j0eval[2]) < 0.0000010000000000)
7724  {
7725  {
7726  IkReal evalcond[1];
7727  bool bgotonextstatement = true;
7728  do
7729  {
7730  IkReal x312 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7731  if (IKabs(x312) == 0)
7732  {
7733  continue;
7734  }
7735  IkReal x310 = pow(x312, -0.5);
7736  IkReal x311 = ((-1.0) * x310);
7737  CheckValue<IkReal> x313 =
7738  IKatan2WithCheck(IkReal(new_r00),
7739  IkReal(((-1.0) * new_r10)),
7740  IKFAST_ATAN2_MAGTHRESH);
7741  if (!x313.valid)
7742  {
7743  continue;
7744  }
7745  IkReal gconst0 = ((-1.0) * (x313.value));
7746  IkReal gconst1 = (new_r00 * x311);
7747  IkReal gconst2 = (new_r10 * x311);
7748  CheckValue<IkReal> x314 =
7749  IKatan2WithCheck(IkReal(new_r00),
7750  IkReal(((-1.0) * new_r10)),
7751  IKFAST_ATAN2_MAGTHRESH);
7752  if (!x314.valid)
7753  {
7754  continue;
7755  }
7756  evalcond[0] =
7757  ((-3.14159265358979) +
7758  (IKfmod(((3.14159265358979) + (IKabs(((x314.value) + j2)))),
7759  6.28318530717959)));
7760  if (IKabs(evalcond[0]) < 0.0000050000000000)
7761  {
7762  bgotonextstatement = false;
7763  {
7764  IkReal j0eval[2];
7765  CheckValue<IkReal> x318 =
7766  IKatan2WithCheck(IkReal(new_r00),
7767  IkReal(((-1.0) * new_r10)),
7768  IKFAST_ATAN2_MAGTHRESH);
7769  if (!x318.valid)
7770  {
7771  continue;
7772  }
7773  IkReal x315 = ((-1.0) * (x318.value));
7774  IkReal x316 = x310;
7775  IkReal x317 = ((-1.0) * x316);
7776  sj1 = 0;
7777  cj1 = 1.0;
7778  j1 = 0;
7779  sj2 = gconst1;
7780  cj2 = gconst2;
7781  j2 = x315;
7782  IkReal gconst0 = x315;
7783  IkReal gconst1 = (new_r00 * x317);
7784  IkReal gconst2 = (new_r10 * x317);
7785  IkReal x319 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7786  j0eval[0] = x319;
7787  j0eval[1] = IKsign(x319);
7788  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7789  IKabs(j0eval[1]) < 0.0000010000000000)
7790  {
7791  {
7792  IkReal j0eval[3];
7793  CheckValue<IkReal> x323 =
7794  IKatan2WithCheck(IkReal(new_r00),
7795  IkReal(((-1.0) * new_r10)),
7796  IKFAST_ATAN2_MAGTHRESH);
7797  if (!x323.valid)
7798  {
7799  continue;
7800  }
7801  IkReal x320 = ((-1.0) * (x323.value));
7802  IkReal x321 = x310;
7803  IkReal x322 = ((-1.0) * x321);
7804  sj1 = 0;
7805  cj1 = 1.0;
7806  j1 = 0;
7807  sj2 = gconst1;
7808  cj2 = gconst2;
7809  j2 = x320;
7810  IkReal gconst0 = x320;
7811  IkReal gconst1 = (new_r00 * x322);
7812  IkReal gconst2 = (new_r10 * x322);
7813  IkReal x324 = new_r10 * new_r10;
7814  IkReal x325 =
7815  (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7816  IkReal x326 = x310;
7817  IkReal x327 = (new_r10 * x326);
7818  j0eval[0] = x325;
7819  j0eval[1] = IKsign(x325);
7820  j0eval[2] =
7821  ((IKabs(((((-1.0) * new_r00 * x327)) +
7822  ((new_r11 * x327))))) +
7823  (IKabs((((new_r01 * x327)) + ((x324 * x326))))));
7824  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7825  IKabs(j0eval[1]) < 0.0000010000000000 ||
7826  IKabs(j0eval[2]) < 0.0000010000000000)
7827  {
7828  {
7829  IkReal j0eval[1];
7830  CheckValue<IkReal> x331 =
7831  IKatan2WithCheck(IkReal(new_r00),
7832  IkReal(((-1.0) * new_r10)),
7833  IKFAST_ATAN2_MAGTHRESH);
7834  if (!x331.valid)
7835  {
7836  continue;
7837  }
7838  IkReal x328 = ((-1.0) * (x331.value));
7839  IkReal x329 = x310;
7840  IkReal x330 = ((-1.0) * x329);
7841  sj1 = 0;
7842  cj1 = 1.0;
7843  j1 = 0;
7844  sj2 = gconst1;
7845  cj2 = gconst2;
7846  j2 = x328;
7847  IkReal gconst0 = x328;
7848  IkReal gconst1 = (new_r00 * x330);
7849  IkReal gconst2 = (new_r10 * x330);
7850  IkReal x332 = new_r10 * new_r10;
7851  IkReal x333 = new_r00 * new_r00;
7852  CheckValue<IkReal> x340 =
7853  IKPowWithIntegerCheck((x333 + x332), -1);
7854  if (!x340.valid)
7855  {
7856  continue;
7857  }
7858  IkReal x334 = x340.value;
7859  IkReal x335 = (x332 * x334);
7860  CheckValue<IkReal> x341 = IKPowWithIntegerCheck(
7861  ((((-1.0) * x332)) + (((-1.0) * x333))), -1);
7862  if (!x341.valid)
7863  {
7864  continue;
7865  }
7866  IkReal x336 = x341.value;
7867  IkReal x337 = ((1.0) * x336);
7868  IkReal x338 = (new_r00 * x337);
7869  IkReal x339 = (new_r10 * x337);
7870  j0eval[0] =
7871  ((IKabs(((((-1.0) * new_r10 * x338)) +
7872  (((-1.0) * x338 *
7873  (new_r10 * new_r10 * new_r10))) +
7874  (((-1.0) * new_r10 * x338 *
7875  (new_r00 * new_r00)))))) +
7876  (IKabs((((x334 * (x333 * x333))) +
7877  (((-1.0) * x335)) + ((x333 * x335))))));
7878  if (IKabs(j0eval[0]) < 0.0000010000000000)
7879  {
7880  continue; // no branches [j0]
7881  }
7882  else
7883  {
7884  {
7885  IkReal j0array[1], cj0array[1], sj0array[1];
7886  bool j0valid[1] = { false };
7887  _nj0 = 1;
7888  CheckValue<IkReal> x342 = IKatan2WithCheck(
7889  IkReal(((((-1.0) * (gconst2 * gconst2))) +
7890  (new_r00 * new_r00))),
7891  IkReal(((((-1.0) * gconst1 * gconst2)) +
7892  (((-1.0) * new_r00 * new_r10)))),
7893  IKFAST_ATAN2_MAGTHRESH);
7894  if (!x342.valid)
7895  {
7896  continue;
7897  }
7898  CheckValue<IkReal> x343 = IKPowWithIntegerCheck(
7899  IKsign((((gconst2 * new_r10)) +
7900  ((gconst1 * new_r00)))),
7901  -1);
7902  if (!x343.valid)
7903  {
7904  continue;
7905  }
7906  j0array[0] = ((-1.5707963267949) + (x342.value) +
7907  (((1.5707963267949) * (x343.value))));
7908  sj0array[0] = IKsin(j0array[0]);
7909  cj0array[0] = IKcos(j0array[0]);
7910  if (j0array[0] > IKPI)
7911  {
7912  j0array[0] -= IK2PI;
7913  }
7914  else if (j0array[0] < -IKPI)
7915  {
7916  j0array[0] += IK2PI;
7917  }
7918  j0valid[0] = true;
7919  for (int ij0 = 0; ij0 < 1; ++ij0)
7920  {
7921  if (!j0valid[ij0])
7922  {
7923  continue;
7924  }
7925  _ij0[0] = ij0;
7926  _ij0[1] = -1;
7927  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
7928  {
7929  if (j0valid[iij0] &&
7930  IKabs(cj0array[ij0] - cj0array[iij0]) <
7931  IKFAST_SOLUTION_THRESH &&
7932  IKabs(sj0array[ij0] - sj0array[iij0]) <
7933  IKFAST_SOLUTION_THRESH)
7934  {
7935  j0valid[iij0] = false;
7936  _ij0[1] = iij0;
7937  break;
7938  }
7939  }
7940  j0 = j0array[ij0];
7941  cj0 = cj0array[ij0];
7942  sj0 = sj0array[ij0];
7943  {
7944  IkReal evalcond[8];
7945  IkReal x344 = IKcos(j0);
7946  IkReal x345 = IKsin(j0);
7947  IkReal x346 = ((1.0) * gconst1);
7948  IkReal x347 = (gconst2 * x344);
7949  IkReal x348 = (gconst1 * x344);
7950  IkReal x349 = (gconst2 * x345);
7951  IkReal x350 = ((1.0) * x345);
7952  IkReal x351 = (x345 * x346);
7953  evalcond[0] = (gconst2 + ((new_r00 * x344)) +
7954  ((new_r10 * x345)));
7955  evalcond[1] = (x348 + x349 + new_r10);
7956  evalcond[2] = (gconst1 + ((new_r10 * x344)) +
7957  (((-1.0) * new_r00 * x350)));
7958  evalcond[3] = (gconst2 + ((new_r11 * x344)) +
7959  (((-1.0) * new_r01 * x350)));
7960  evalcond[4] =
7961  ((((-1.0) * x351)) + x347 + new_r00);
7962  evalcond[5] =
7963  ((((-1.0) * x351)) + x347 + new_r11);
7964  evalcond[6] =
7965  (((new_r11 * x345)) + ((new_r01 * x344)) +
7966  (((-1.0) * x346)));
7967  evalcond[7] = ((((-1.0) * x344 * x346)) +
7968  new_r01 + (((-1.0) * x349)));
7969  if (IKabs(evalcond[0]) >
7970  IKFAST_EVALCOND_THRESH ||
7971  IKabs(evalcond[1]) >
7972  IKFAST_EVALCOND_THRESH ||
7973  IKabs(evalcond[2]) >
7974  IKFAST_EVALCOND_THRESH ||
7975  IKabs(evalcond[3]) >
7976  IKFAST_EVALCOND_THRESH ||
7977  IKabs(evalcond[4]) >
7978  IKFAST_EVALCOND_THRESH ||
7979  IKabs(evalcond[5]) >
7980  IKFAST_EVALCOND_THRESH ||
7981  IKabs(evalcond[6]) >
7982  IKFAST_EVALCOND_THRESH ||
7983  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
7984  {
7985  continue;
7986  }
7987  }
7988 
7989  {
7990  std::vector<IkSingleDOFSolutionBase<IkReal> >
7991  vinfos(7);
7992  vinfos[0].jointtype = 1;
7993  vinfos[0].foffset = j0;
7994  vinfos[0].indices[0] = _ij0[0];
7995  vinfos[0].indices[1] = _ij0[1];
7996  vinfos[0].maxsolutions = _nj0;
7997  vinfos[1].jointtype = 1;
7998  vinfos[1].foffset = j1;
7999  vinfos[1].indices[0] = _ij1[0];
8000  vinfos[1].indices[1] = _ij1[1];
8001  vinfos[1].maxsolutions = _nj1;
8002  vinfos[2].jointtype = 1;
8003  vinfos[2].foffset = j2;
8004  vinfos[2].indices[0] = _ij2[0];
8005  vinfos[2].indices[1] = _ij2[1];
8006  vinfos[2].maxsolutions = _nj2;
8007  vinfos[3].jointtype = 1;
8008  vinfos[3].foffset = j3;
8009  vinfos[3].indices[0] = _ij3[0];
8010  vinfos[3].indices[1] = _ij3[1];
8011  vinfos[3].maxsolutions = _nj3;
8012  vinfos[4].jointtype = 1;
8013  vinfos[4].foffset = j4;
8014  vinfos[4].indices[0] = _ij4[0];
8015  vinfos[4].indices[1] = _ij4[1];
8016  vinfos[4].maxsolutions = _nj4;
8017  vinfos[5].jointtype = 1;
8018  vinfos[5].foffset = j5;
8019  vinfos[5].indices[0] = _ij5[0];
8020  vinfos[5].indices[1] = _ij5[1];
8021  vinfos[5].maxsolutions = _nj5;
8022  vinfos[6].jointtype = 1;
8023  vinfos[6].foffset = j6;
8024  vinfos[6].indices[0] = _ij6[0];
8025  vinfos[6].indices[1] = _ij6[1];
8026  vinfos[6].maxsolutions = _nj6;
8027  std::vector<int> vfree(0);
8028  solutions.AddSolution(vinfos, vfree);
8029  }
8030  }
8031  }
8032  }
8033  }
8034  }
8035  else
8036  {
8037  {
8038  IkReal j0array[1], cj0array[1], sj0array[1];
8039  bool j0valid[1] = { false };
8040  _nj0 = 1;
8041  IkReal x352 = ((1.0) * gconst2);
8042  CheckValue<IkReal> x353 = IKatan2WithCheck(
8043  IkReal((((gconst2 * new_r00)) +
8044  (((-1.0) * new_r11 * x352)))),
8045  IkReal(((((-1.0) * new_r01 * x352)) +
8046  (((-1.0) * new_r10 * x352)))),
8047  IKFAST_ATAN2_MAGTHRESH);
8048  if (!x353.valid)
8049  {
8050  continue;
8051  }
8052  CheckValue<IkReal> x354 = IKPowWithIntegerCheck(
8053  IKsign((((new_r10 * new_r11)) +
8054  ((new_r00 * new_r01)))),
8055  -1);
8056  if (!x354.valid)
8057  {
8058  continue;
8059  }
8060  j0array[0] = ((-1.5707963267949) + (x353.value) +
8061  (((1.5707963267949) * (x354.value))));
8062  sj0array[0] = IKsin(j0array[0]);
8063  cj0array[0] = IKcos(j0array[0]);
8064  if (j0array[0] > IKPI)
8065  {
8066  j0array[0] -= IK2PI;
8067  }
8068  else if (j0array[0] < -IKPI)
8069  {
8070  j0array[0] += IK2PI;
8071  }
8072  j0valid[0] = true;
8073  for (int ij0 = 0; ij0 < 1; ++ij0)
8074  {
8075  if (!j0valid[ij0])
8076  {
8077  continue;
8078  }
8079  _ij0[0] = ij0;
8080  _ij0[1] = -1;
8081  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8082  {
8083  if (j0valid[iij0] &&
8084  IKabs(cj0array[ij0] - cj0array[iij0]) <
8085  IKFAST_SOLUTION_THRESH &&
8086  IKabs(sj0array[ij0] - sj0array[iij0]) <
8087  IKFAST_SOLUTION_THRESH)
8088  {
8089  j0valid[iij0] = false;
8090  _ij0[1] = iij0;
8091  break;
8092  }
8093  }
8094  j0 = j0array[ij0];
8095  cj0 = cj0array[ij0];
8096  sj0 = sj0array[ij0];
8097  {
8098  IkReal evalcond[8];
8099  IkReal x355 = IKcos(j0);
8100  IkReal x356 = IKsin(j0);
8101  IkReal x357 = ((1.0) * gconst1);
8102  IkReal x358 = (gconst2 * x355);
8103  IkReal x359 = (gconst1 * x355);
8104  IkReal x360 = (gconst2 * x356);
8105  IkReal x361 = ((1.0) * x356);
8106  IkReal x362 = (x356 * x357);
8107  evalcond[0] = (gconst2 + ((new_r10 * x356)) +
8108  ((new_r00 * x355)));
8109  evalcond[1] = (x359 + x360 + new_r10);
8110  evalcond[2] = ((((-1.0) * new_r00 * x361)) +
8111  gconst1 + ((new_r10 * x355)));
8112  evalcond[3] = (gconst2 + ((new_r11 * x355)) +
8113  (((-1.0) * new_r01 * x361)));
8114  evalcond[4] = ((((-1.0) * x362)) + x358 + new_r00);
8115  evalcond[5] = ((((-1.0) * x362)) + x358 + new_r11);
8116  evalcond[6] =
8117  (((new_r01 * x355)) + (((-1.0) * x357)) +
8118  ((new_r11 * x356)));
8119  evalcond[7] = ((((-1.0) * x360)) + new_r01 +
8120  (((-1.0) * x355 * x357)));
8121  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8122  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8123  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8124  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8125  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8126  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8127  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8128  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8129  {
8130  continue;
8131  }
8132  }
8133 
8134  {
8135  std::vector<IkSingleDOFSolutionBase<IkReal> >
8136  vinfos(7);
8137  vinfos[0].jointtype = 1;
8138  vinfos[0].foffset = j0;
8139  vinfos[0].indices[0] = _ij0[0];
8140  vinfos[0].indices[1] = _ij0[1];
8141  vinfos[0].maxsolutions = _nj0;
8142  vinfos[1].jointtype = 1;
8143  vinfos[1].foffset = j1;
8144  vinfos[1].indices[0] = _ij1[0];
8145  vinfos[1].indices[1] = _ij1[1];
8146  vinfos[1].maxsolutions = _nj1;
8147  vinfos[2].jointtype = 1;
8148  vinfos[2].foffset = j2;
8149  vinfos[2].indices[0] = _ij2[0];
8150  vinfos[2].indices[1] = _ij2[1];
8151  vinfos[2].maxsolutions = _nj2;
8152  vinfos[3].jointtype = 1;
8153  vinfos[3].foffset = j3;
8154  vinfos[3].indices[0] = _ij3[0];
8155  vinfos[3].indices[1] = _ij3[1];
8156  vinfos[3].maxsolutions = _nj3;
8157  vinfos[4].jointtype = 1;
8158  vinfos[4].foffset = j4;
8159  vinfos[4].indices[0] = _ij4[0];
8160  vinfos[4].indices[1] = _ij4[1];
8161  vinfos[4].maxsolutions = _nj4;
8162  vinfos[5].jointtype = 1;
8163  vinfos[5].foffset = j5;
8164  vinfos[5].indices[0] = _ij5[0];
8165  vinfos[5].indices[1] = _ij5[1];
8166  vinfos[5].maxsolutions = _nj5;
8167  vinfos[6].jointtype = 1;
8168  vinfos[6].foffset = j6;
8169  vinfos[6].indices[0] = _ij6[0];
8170  vinfos[6].indices[1] = _ij6[1];
8171  vinfos[6].maxsolutions = _nj6;
8172  std::vector<int> vfree(0);
8173  solutions.AddSolution(vinfos, vfree);
8174  }
8175  }
8176  }
8177  }
8178  }
8179  }
8180  else
8181  {
8182  {
8183  IkReal j0array[1], cj0array[1], sj0array[1];
8184  bool j0valid[1] = { false };
8185  _nj0 = 1;
8186  IkReal x363 = ((1.0) * new_r10);
8187  CheckValue<IkReal> x364 = IKPowWithIntegerCheck(
8188  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8189  -1);
8190  if (!x364.valid)
8191  {
8192  continue;
8193  }
8194  CheckValue<IkReal> x365 = IKatan2WithCheck(
8195  IkReal((((gconst1 * new_r00)) +
8196  (((-1.0) * gconst2 * x363)))),
8197  IkReal(((((-1.0) * gconst1 * x363)) +
8198  (((-1.0) * gconst2 * new_r00)))),
8199  IKFAST_ATAN2_MAGTHRESH);
8200  if (!x365.valid)
8201  {
8202  continue;
8203  }
8204  j0array[0] =
8205  ((-1.5707963267949) +
8206  (((1.5707963267949) * (x364.value))) + (x365.value));
8207  sj0array[0] = IKsin(j0array[0]);
8208  cj0array[0] = IKcos(j0array[0]);
8209  if (j0array[0] > IKPI)
8210  {
8211  j0array[0] -= IK2PI;
8212  }
8213  else if (j0array[0] < -IKPI)
8214  {
8215  j0array[0] += IK2PI;
8216  }
8217  j0valid[0] = true;
8218  for (int ij0 = 0; ij0 < 1; ++ij0)
8219  {
8220  if (!j0valid[ij0])
8221  {
8222  continue;
8223  }
8224  _ij0[0] = ij0;
8225  _ij0[1] = -1;
8226  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8227  {
8228  if (j0valid[iij0] &&
8229  IKabs(cj0array[ij0] - cj0array[iij0]) <
8230  IKFAST_SOLUTION_THRESH &&
8231  IKabs(sj0array[ij0] - sj0array[iij0]) <
8232  IKFAST_SOLUTION_THRESH)
8233  {
8234  j0valid[iij0] = false;
8235  _ij0[1] = iij0;
8236  break;
8237  }
8238  }
8239  j0 = j0array[ij0];
8240  cj0 = cj0array[ij0];
8241  sj0 = sj0array[ij0];
8242  {
8243  IkReal evalcond[8];
8244  IkReal x366 = IKcos(j0);
8245  IkReal x367 = IKsin(j0);
8246  IkReal x368 = ((1.0) * gconst1);
8247  IkReal x369 = (gconst2 * x366);
8248  IkReal x370 = (gconst1 * x366);
8249  IkReal x371 = (gconst2 * x367);
8250  IkReal x372 = ((1.0) * x367);
8251  IkReal x373 = (x367 * x368);
8252  evalcond[0] =
8253  (gconst2 + ((new_r00 * x366)) + ((new_r10 * x367)));
8254  evalcond[1] = (x371 + x370 + new_r10);
8255  evalcond[2] = ((((-1.0) * new_r00 * x372)) + gconst1 +
8256  ((new_r10 * x366)));
8257  evalcond[3] = (gconst2 + (((-1.0) * new_r01 * x372)) +
8258  ((new_r11 * x366)));
8259  evalcond[4] = ((((-1.0) * x373)) + x369 + new_r00);
8260  evalcond[5] = ((((-1.0) * x373)) + x369 + new_r11);
8261  evalcond[6] = ((((-1.0) * x368)) + ((new_r01 * x366)) +
8262  ((new_r11 * x367)));
8263  evalcond[7] = ((((-1.0) * x371)) +
8264  (((-1.0) * x366 * x368)) + new_r01);
8265  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8266  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8267  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8268  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8269  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8270  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8271  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8272  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8273  {
8274  continue;
8275  }
8276  }
8277 
8278  {
8279  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8280  vinfos[0].jointtype = 1;
8281  vinfos[0].foffset = j0;
8282  vinfos[0].indices[0] = _ij0[0];
8283  vinfos[0].indices[1] = _ij0[1];
8284  vinfos[0].maxsolutions = _nj0;
8285  vinfos[1].jointtype = 1;
8286  vinfos[1].foffset = j1;
8287  vinfos[1].indices[0] = _ij1[0];
8288  vinfos[1].indices[1] = _ij1[1];
8289  vinfos[1].maxsolutions = _nj1;
8290  vinfos[2].jointtype = 1;
8291  vinfos[2].foffset = j2;
8292  vinfos[2].indices[0] = _ij2[0];
8293  vinfos[2].indices[1] = _ij2[1];
8294  vinfos[2].maxsolutions = _nj2;
8295  vinfos[3].jointtype = 1;
8296  vinfos[3].foffset = j3;
8297  vinfos[3].indices[0] = _ij3[0];
8298  vinfos[3].indices[1] = _ij3[1];
8299  vinfos[3].maxsolutions = _nj3;
8300  vinfos[4].jointtype = 1;
8301  vinfos[4].foffset = j4;
8302  vinfos[4].indices[0] = _ij4[0];
8303  vinfos[4].indices[1] = _ij4[1];
8304  vinfos[4].maxsolutions = _nj4;
8305  vinfos[5].jointtype = 1;
8306  vinfos[5].foffset = j5;
8307  vinfos[5].indices[0] = _ij5[0];
8308  vinfos[5].indices[1] = _ij5[1];
8309  vinfos[5].maxsolutions = _nj5;
8310  vinfos[6].jointtype = 1;
8311  vinfos[6].foffset = j6;
8312  vinfos[6].indices[0] = _ij6[0];
8313  vinfos[6].indices[1] = _ij6[1];
8314  vinfos[6].maxsolutions = _nj6;
8315  std::vector<int> vfree(0);
8316  solutions.AddSolution(vinfos, vfree);
8317  }
8318  }
8319  }
8320  }
8321  }
8322  }
8323  } while (0);
8324  if (bgotonextstatement)
8325  {
8326  bool bgotonextstatement = true;
8327  do
8328  {
8329  IkReal x376 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8330  if (IKabs(x376) == 0)
8331  {
8332  continue;
8333  }
8334  IkReal x374 = pow(x376, -0.5);
8335  IkReal x375 = ((1.0) * x374);
8336  CheckValue<IkReal> x377 =
8337  IKatan2WithCheck(IkReal(new_r00),
8338  IkReal(((-1.0) * new_r10)),
8339  IKFAST_ATAN2_MAGTHRESH);
8340  if (!x377.valid)
8341  {
8342  continue;
8343  }
8344  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * (x377.value))));
8345  IkReal gconst4 = (new_r00 * x375);
8346  IkReal gconst5 = (new_r10 * x375);
8347  CheckValue<IkReal> x378 =
8348  IKatan2WithCheck(IkReal(new_r00),
8349  IkReal(((-1.0) * new_r10)),
8350  IKFAST_ATAN2_MAGTHRESH);
8351  if (!x378.valid)
8352  {
8353  continue;
8354  }
8355  evalcond[0] =
8356  ((-3.14159265358979) +
8357  (IKfmod(((3.14159265358979) +
8358  (IKabs(((-3.14159265358979) + (x378.value) + j2)))),
8359  6.28318530717959)));
8360  if (IKabs(evalcond[0]) < 0.0000050000000000)
8361  {
8362  bgotonextstatement = false;
8363  {
8364  IkReal j0eval[2];
8365  CheckValue<IkReal> x382 =
8366  IKatan2WithCheck(IkReal(new_r00),
8367  IkReal(((-1.0) * new_r10)),
8368  IKFAST_ATAN2_MAGTHRESH);
8369  if (!x382.valid)
8370  {
8371  continue;
8372  }
8373  IkReal x379 = ((1.0) * (x382.value));
8374  IkReal x380 = x374;
8375  IkReal x381 = ((1.0) * x380);
8376  sj1 = 0;
8377  cj1 = 1.0;
8378  j1 = 0;
8379  sj2 = gconst4;
8380  cj2 = gconst5;
8381  j2 = ((3.14159265) + (((-1.0) * x379)));
8382  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x379)));
8383  IkReal gconst4 = (new_r00 * x381);
8384  IkReal gconst5 = (new_r10 * x381);
8385  IkReal x383 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8386  j0eval[0] = x383;
8387  j0eval[1] = IKsign(x383);
8388  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8389  IKabs(j0eval[1]) < 0.0000010000000000)
8390  {
8391  {
8392  IkReal j0eval[3];
8393  CheckValue<IkReal> x387 =
8394  IKatan2WithCheck(IkReal(new_r00),
8395  IkReal(((-1.0) * new_r10)),
8396  IKFAST_ATAN2_MAGTHRESH);
8397  if (!x387.valid)
8398  {
8399  continue;
8400  }
8401  IkReal x384 = ((1.0) * (x387.value));
8402  IkReal x385 = x374;
8403  IkReal x386 = ((1.0) * x385);
8404  sj1 = 0;
8405  cj1 = 1.0;
8406  j1 = 0;
8407  sj2 = gconst4;
8408  cj2 = gconst5;
8409  j2 = ((3.14159265) + (((-1.0) * x384)));
8410  IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x384)));
8411  IkReal gconst4 = (new_r00 * x386);
8412  IkReal gconst5 = (new_r10 * x386);
8413  IkReal x388 = new_r10 * new_r10;
8414  IkReal x389 = (new_r10 * new_r11);
8415  IkReal x390 = (((new_r00 * new_r01)) + x389);
8416  IkReal x391 = x374;
8417  IkReal x392 = ((1.0) * x391);
8418  j0eval[0] = x390;
8419  j0eval[1] =
8420  ((IKabs(((((-1.0) * new_r01 * new_r10 * x392)) +
8421  (((-1.0) * x388 * x392))))) +
8422  (IKabs(((((-1.0) * x389 * x392)) +
8423  ((new_r00 * new_r10 * x391))))));
8424  j0eval[2] = IKsign(x390);
8425  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8426  IKabs(j0eval[1]) < 0.0000010000000000 ||
8427  IKabs(j0eval[2]) < 0.0000010000000000)
8428  {
8429  {
8430  IkReal j0eval[1];
8431  CheckValue<IkReal> x396 =
8432  IKatan2WithCheck(IkReal(new_r00),
8433  IkReal(((-1.0) * new_r10)),
8434  IKFAST_ATAN2_MAGTHRESH);
8435  if (!x396.valid)
8436  {
8437  continue;
8438  }
8439  IkReal x393 = ((1.0) * (x396.value));
8440  IkReal x394 = x374;
8441  IkReal x395 = ((1.0) * x394);
8442  sj1 = 0;
8443  cj1 = 1.0;
8444  j1 = 0;
8445  sj2 = gconst4;
8446  cj2 = gconst5;
8447  j2 = ((3.14159265) + (((-1.0) * x393)));
8448  IkReal gconst3 =
8449  ((3.14159265358979) + (((-1.0) * x393)));
8450  IkReal gconst4 = (new_r00 * x395);
8451  IkReal gconst5 = (new_r10 * x395);
8452  IkReal x397 = new_r10 * new_r10;
8453  IkReal x398 = new_r00 * new_r00;
8454  CheckValue<IkReal> x405 =
8455  IKPowWithIntegerCheck((x397 + x398), -1);
8456  if (!x405.valid)
8457  {
8458  continue;
8459  }
8460  IkReal x399 = x405.value;
8461  IkReal x400 = (x397 * x399);
8462  CheckValue<IkReal> x406 = IKPowWithIntegerCheck(
8463  ((((-1.0) * x398)) + (((-1.0) * x397))), -1);
8464  if (!x406.valid)
8465  {
8466  continue;
8467  }
8468  IkReal x401 = x406.value;
8469  IkReal x402 = ((1.0) * x401);
8470  IkReal x403 = (new_r00 * x402);
8471  IkReal x404 = (new_r10 * x402);
8472  j0eval[0] =
8473  ((IKabs((((x399 * (x398 * x398))) +
8474  ((x398 * x400)) + (((-1.0) * x400))))) +
8475  (IKabs(((((-1.0) * x403 *
8476  (new_r10 * new_r10 * new_r10))) +
8477  (((-1.0) * new_r10 * x403)) +
8478  (((-1.0) * new_r10 * x403 *
8479  (new_r00 * new_r00)))))));
8480  if (IKabs(j0eval[0]) < 0.0000010000000000)
8481  {
8482  continue; // no branches [j0]
8483  }
8484  else
8485  {
8486  {
8487  IkReal j0array[1], cj0array[1], sj0array[1];
8488  bool j0valid[1] = { false };
8489  _nj0 = 1;
8490  CheckValue<IkReal> x407 = IKPowWithIntegerCheck(
8491  IKsign((((gconst4 * new_r00)) +
8492  ((gconst5 * new_r10)))),
8493  -1);
8494  if (!x407.valid)
8495  {
8496  continue;
8497  }
8498  CheckValue<IkReal> x408 = IKatan2WithCheck(
8499  IkReal(((((-1.0) * (gconst5 * gconst5))) +
8500  (new_r00 * new_r00))),
8501  IkReal(((((-1.0) * gconst4 * gconst5)) +
8502  (((-1.0) * new_r00 * new_r10)))),
8503  IKFAST_ATAN2_MAGTHRESH);
8504  if (!x408.valid)
8505  {
8506  continue;
8507  }
8508  j0array[0] =
8509  ((-1.5707963267949) +
8510  (((1.5707963267949) * (x407.value))) +
8511  (x408.value));
8512  sj0array[0] = IKsin(j0array[0]);
8513  cj0array[0] = IKcos(j0array[0]);
8514  if (j0array[0] > IKPI)
8515  {
8516  j0array[0] -= IK2PI;
8517  }
8518  else if (j0array[0] < -IKPI)
8519  {
8520  j0array[0] += IK2PI;
8521  }
8522  j0valid[0] = true;
8523  for (int ij0 = 0; ij0 < 1; ++ij0)
8524  {
8525  if (!j0valid[ij0])
8526  {
8527  continue;
8528  }
8529  _ij0[0] = ij0;
8530  _ij0[1] = -1;
8531  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8532  {
8533  if (j0valid[iij0] &&
8534  IKabs(cj0array[ij0] - cj0array[iij0]) <
8535  IKFAST_SOLUTION_THRESH &&
8536  IKabs(sj0array[ij0] - sj0array[iij0]) <
8537  IKFAST_SOLUTION_THRESH)
8538  {
8539  j0valid[iij0] = false;
8540  _ij0[1] = iij0;
8541  break;
8542  }
8543  }
8544  j0 = j0array[ij0];
8545  cj0 = cj0array[ij0];
8546  sj0 = sj0array[ij0];
8547  {
8548  IkReal evalcond[8];
8549  IkReal x409 = IKcos(j0);
8550  IkReal x410 = IKsin(j0);
8551  IkReal x411 = ((1.0) * gconst4);
8552  IkReal x412 = (gconst5 * x409);
8553  IkReal x413 = ((1.0) * x410);
8554  IkReal x414 = (x410 * x411);
8555  evalcond[0] = (gconst5 + ((new_r10 * x410)) +
8556  ((new_r00 * x409)));
8557  evalcond[1] = (((gconst5 * x410)) +
8558  ((gconst4 * x409)) + new_r10);
8559  evalcond[2] = (gconst4 + ((new_r10 * x409)) +
8560  (((-1.0) * new_r00 * x413)));
8561  evalcond[3] = (gconst5 + ((new_r11 * x409)) +
8562  (((-1.0) * new_r01 * x413)));
8563  evalcond[4] =
8564  (x412 + new_r00 + (((-1.0) * x414)));
8565  evalcond[5] =
8566  (x412 + new_r11 + (((-1.0) * x414)));
8567  evalcond[6] =
8568  (((new_r11 * x410)) + ((new_r01 * x409)) +
8569  (((-1.0) * x411)));
8570  evalcond[7] =
8571  ((((-1.0) * gconst5 * x413)) + new_r01 +
8572  (((-1.0) * x409 * x411)));
8573  if (IKabs(evalcond[0]) >
8574  IKFAST_EVALCOND_THRESH ||
8575  IKabs(evalcond[1]) >
8576  IKFAST_EVALCOND_THRESH ||
8577  IKabs(evalcond[2]) >
8578  IKFAST_EVALCOND_THRESH ||
8579  IKabs(evalcond[3]) >
8580  IKFAST_EVALCOND_THRESH ||
8581  IKabs(evalcond[4]) >
8582  IKFAST_EVALCOND_THRESH ||
8583  IKabs(evalcond[5]) >
8584  IKFAST_EVALCOND_THRESH ||
8585  IKabs(evalcond[6]) >
8586  IKFAST_EVALCOND_THRESH ||
8587  IKabs(evalcond[7]) >
8588  IKFAST_EVALCOND_THRESH)
8589  {
8590  continue;
8591  }
8592  }
8593 
8594  {
8595  std::vector<IkSingleDOFSolutionBase<IkReal> >
8596  vinfos(7);
8597  vinfos[0].jointtype = 1;
8598  vinfos[0].foffset = j0;
8599  vinfos[0].indices[0] = _ij0[0];
8600  vinfos[0].indices[1] = _ij0[1];
8601  vinfos[0].maxsolutions = _nj0;
8602  vinfos[1].jointtype = 1;
8603  vinfos[1].foffset = j1;
8604  vinfos[1].indices[0] = _ij1[0];
8605  vinfos[1].indices[1] = _ij1[1];
8606  vinfos[1].maxsolutions = _nj1;
8607  vinfos[2].jointtype = 1;
8608  vinfos[2].foffset = j2;
8609  vinfos[2].indices[0] = _ij2[0];
8610  vinfos[2].indices[1] = _ij2[1];
8611  vinfos[2].maxsolutions = _nj2;
8612  vinfos[3].jointtype = 1;
8613  vinfos[3].foffset = j3;
8614  vinfos[3].indices[0] = _ij3[0];
8615  vinfos[3].indices[1] = _ij3[1];
8616  vinfos[3].maxsolutions = _nj3;
8617  vinfos[4].jointtype = 1;
8618  vinfos[4].foffset = j4;
8619  vinfos[4].indices[0] = _ij4[0];
8620  vinfos[4].indices[1] = _ij4[1];
8621  vinfos[4].maxsolutions = _nj4;
8622  vinfos[5].jointtype = 1;
8623  vinfos[5].foffset = j5;
8624  vinfos[5].indices[0] = _ij5[0];
8625  vinfos[5].indices[1] = _ij5[1];
8626  vinfos[5].maxsolutions = _nj5;
8627  vinfos[6].jointtype = 1;
8628  vinfos[6].foffset = j6;
8629  vinfos[6].indices[0] = _ij6[0];
8630  vinfos[6].indices[1] = _ij6[1];
8631  vinfos[6].maxsolutions = _nj6;
8632  std::vector<int> vfree(0);
8633  solutions.AddSolution(vinfos, vfree);
8634  }
8635  }
8636  }
8637  }
8638  }
8639  }
8640  else
8641  {
8642  {
8643  IkReal j0array[1], cj0array[1], sj0array[1];
8644  bool j0valid[1] = { false };
8645  _nj0 = 1;
8646  IkReal x415 = ((1.0) * gconst5);
8647  CheckValue<IkReal> x416 = IKatan2WithCheck(
8648  IkReal((((gconst5 * new_r00)) +
8649  (((-1.0) * new_r11 * x415)))),
8650  IkReal(((((-1.0) * new_r10 * x415)) +
8651  (((-1.0) * new_r01 * x415)))),
8652  IKFAST_ATAN2_MAGTHRESH);
8653  if (!x416.valid)
8654  {
8655  continue;
8656  }
8657  CheckValue<IkReal> x417 = IKPowWithIntegerCheck(
8658  IKsign((((new_r10 * new_r11)) +
8659  ((new_r00 * new_r01)))),
8660  -1);
8661  if (!x417.valid)
8662  {
8663  continue;
8664  }
8665  j0array[0] = ((-1.5707963267949) + (x416.value) +
8666  (((1.5707963267949) * (x417.value))));
8667  sj0array[0] = IKsin(j0array[0]);
8668  cj0array[0] = IKcos(j0array[0]);
8669  if (j0array[0] > IKPI)
8670  {
8671  j0array[0] -= IK2PI;
8672  }
8673  else if (j0array[0] < -IKPI)
8674  {
8675  j0array[0] += IK2PI;
8676  }
8677  j0valid[0] = true;
8678  for (int ij0 = 0; ij0 < 1; ++ij0)
8679  {
8680  if (!j0valid[ij0])
8681  {
8682  continue;
8683  }
8684  _ij0[0] = ij0;
8685  _ij0[1] = -1;
8686  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8687  {
8688  if (j0valid[iij0] &&
8689  IKabs(cj0array[ij0] - cj0array[iij0]) <
8690  IKFAST_SOLUTION_THRESH &&
8691  IKabs(sj0array[ij0] - sj0array[iij0]) <
8692  IKFAST_SOLUTION_THRESH)
8693  {
8694  j0valid[iij0] = false;
8695  _ij0[1] = iij0;
8696  break;
8697  }
8698  }
8699  j0 = j0array[ij0];
8700  cj0 = cj0array[ij0];
8701  sj0 = sj0array[ij0];
8702  {
8703  IkReal evalcond[8];
8704  IkReal x418 = IKcos(j0);
8705  IkReal x419 = IKsin(j0);
8706  IkReal x420 = ((1.0) * gconst4);
8707  IkReal x421 = (gconst5 * x418);
8708  IkReal x422 = ((1.0) * x419);
8709  IkReal x423 = (x419 * x420);
8710  evalcond[0] = (gconst5 + ((new_r10 * x419)) +
8711  ((new_r00 * x418)));
8712  evalcond[1] = (((gconst5 * x419)) + new_r10 +
8713  ((gconst4 * x418)));
8714  evalcond[2] = ((((-1.0) * new_r00 * x422)) +
8715  gconst4 + ((new_r10 * x418)));
8716  evalcond[3] = (gconst5 + ((new_r11 * x418)) +
8717  (((-1.0) * new_r01 * x422)));
8718  evalcond[4] =
8719  ((((-1.0) * x423)) + x421 + new_r00);
8720  evalcond[5] =
8721  ((((-1.0) * x423)) + x421 + new_r11);
8722  evalcond[6] =
8723  ((((-1.0) * x420)) + ((new_r11 * x419)) +
8724  ((new_r01 * x418)));
8725  evalcond[7] =
8726  ((((-1.0) * x418 * x420)) + new_r01 +
8727  (((-1.0) * gconst5 * x422)));
8728  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8729  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8730  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8731  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8732  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8733  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8734  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8735  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8736  {
8737  continue;
8738  }
8739  }
8740 
8741  {
8742  std::vector<IkSingleDOFSolutionBase<IkReal> >
8743  vinfos(7);
8744  vinfos[0].jointtype = 1;
8745  vinfos[0].foffset = j0;
8746  vinfos[0].indices[0] = _ij0[0];
8747  vinfos[0].indices[1] = _ij0[1];
8748  vinfos[0].maxsolutions = _nj0;
8749  vinfos[1].jointtype = 1;
8750  vinfos[1].foffset = j1;
8751  vinfos[1].indices[0] = _ij1[0];
8752  vinfos[1].indices[1] = _ij1[1];
8753  vinfos[1].maxsolutions = _nj1;
8754  vinfos[2].jointtype = 1;
8755  vinfos[2].foffset = j2;
8756  vinfos[2].indices[0] = _ij2[0];
8757  vinfos[2].indices[1] = _ij2[1];
8758  vinfos[2].maxsolutions = _nj2;
8759  vinfos[3].jointtype = 1;
8760  vinfos[3].foffset = j3;
8761  vinfos[3].indices[0] = _ij3[0];
8762  vinfos[3].indices[1] = _ij3[1];
8763  vinfos[3].maxsolutions = _nj3;
8764  vinfos[4].jointtype = 1;
8765  vinfos[4].foffset = j4;
8766  vinfos[4].indices[0] = _ij4[0];
8767  vinfos[4].indices[1] = _ij4[1];
8768  vinfos[4].maxsolutions = _nj4;
8769  vinfos[5].jointtype = 1;
8770  vinfos[5].foffset = j5;
8771  vinfos[5].indices[0] = _ij5[0];
8772  vinfos[5].indices[1] = _ij5[1];
8773  vinfos[5].maxsolutions = _nj5;
8774  vinfos[6].jointtype = 1;
8775  vinfos[6].foffset = j6;
8776  vinfos[6].indices[0] = _ij6[0];
8777  vinfos[6].indices[1] = _ij6[1];
8778  vinfos[6].maxsolutions = _nj6;
8779  std::vector<int> vfree(0);
8780  solutions.AddSolution(vinfos, vfree);
8781  }
8782  }
8783  }
8784  }
8785  }
8786  }
8787  else
8788  {
8789  {
8790  IkReal j0array[1], cj0array[1], sj0array[1];
8791  bool j0valid[1] = { false };
8792  _nj0 = 1;
8793  IkReal x424 = ((1.0) * new_r10);
8794  CheckValue<IkReal> x425 = IKatan2WithCheck(
8795  IkReal((((gconst4 * new_r00)) +
8796  (((-1.0) * gconst5 * x424)))),
8797  IkReal(((((-1.0) * gconst4 * x424)) +
8798  (((-1.0) * gconst5 * new_r00)))),
8799  IKFAST_ATAN2_MAGTHRESH);
8800  if (!x425.valid)
8801  {
8802  continue;
8803  }
8804  CheckValue<IkReal> x426 = IKPowWithIntegerCheck(
8805  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8806  -1);
8807  if (!x426.valid)
8808  {
8809  continue;
8810  }
8811  j0array[0] = ((-1.5707963267949) + (x425.value) +
8812  (((1.5707963267949) * (x426.value))));
8813  sj0array[0] = IKsin(j0array[0]);
8814  cj0array[0] = IKcos(j0array[0]);
8815  if (j0array[0] > IKPI)
8816  {
8817  j0array[0] -= IK2PI;
8818  }
8819  else if (j0array[0] < -IKPI)
8820  {
8821  j0array[0] += IK2PI;
8822  }
8823  j0valid[0] = true;
8824  for (int ij0 = 0; ij0 < 1; ++ij0)
8825  {
8826  if (!j0valid[ij0])
8827  {
8828  continue;
8829  }
8830  _ij0[0] = ij0;
8831  _ij0[1] = -1;
8832  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8833  {
8834  if (j0valid[iij0] &&
8835  IKabs(cj0array[ij0] - cj0array[iij0]) <
8836  IKFAST_SOLUTION_THRESH &&
8837  IKabs(sj0array[ij0] - sj0array[iij0]) <
8838  IKFAST_SOLUTION_THRESH)
8839  {
8840  j0valid[iij0] = false;
8841  _ij0[1] = iij0;
8842  break;
8843  }
8844  }
8845  j0 = j0array[ij0];
8846  cj0 = cj0array[ij0];
8847  sj0 = sj0array[ij0];
8848  {
8849  IkReal evalcond[8];
8850  IkReal x427 = IKcos(j0);
8851  IkReal x428 = IKsin(j0);
8852  IkReal x429 = ((1.0) * gconst4);
8853  IkReal x430 = (gconst5 * x427);
8854  IkReal x431 = ((1.0) * x428);
8855  IkReal x432 = (x428 * x429);
8856  evalcond[0] = (gconst5 + ((new_r10 * x428)) +
8857  ((new_r00 * x427)));
8858  evalcond[1] = (((gconst4 * x427)) +
8859  ((gconst5 * x428)) + new_r10);
8860  evalcond[2] = ((((-1.0) * new_r00 * x431)) + gconst4 +
8861  ((new_r10 * x427)));
8862  evalcond[3] = ((((-1.0) * new_r01 * x431)) + gconst5 +
8863  ((new_r11 * x427)));
8864  evalcond[4] = ((((-1.0) * x432)) + x430 + new_r00);
8865  evalcond[5] = ((((-1.0) * x432)) + x430 + new_r11);
8866  evalcond[6] =
8867  ((((-1.0) * x429)) + ((new_r11 * x428)) +
8868  ((new_r01 * x427)));
8869  evalcond[7] = ((((-1.0) * x427 * x429)) +
8870  (((-1.0) * gconst5 * x431)) + new_r01);
8871  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8872  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8873  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8874  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8875  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8876  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8877  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8878  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8879  {
8880  continue;
8881  }
8882  }
8883 
8884  {
8885  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
8886  7);
8887  vinfos[0].jointtype = 1;
8888  vinfos[0].foffset = j0;
8889  vinfos[0].indices[0] = _ij0[0];
8890  vinfos[0].indices[1] = _ij0[1];
8891  vinfos[0].maxsolutions = _nj0;
8892  vinfos[1].jointtype = 1;
8893  vinfos[1].foffset = j1;
8894  vinfos[1].indices[0] = _ij1[0];
8895  vinfos[1].indices[1] = _ij1[1];
8896  vinfos[1].maxsolutions = _nj1;
8897  vinfos[2].jointtype = 1;
8898  vinfos[2].foffset = j2;
8899  vinfos[2].indices[0] = _ij2[0];
8900  vinfos[2].indices[1] = _ij2[1];
8901  vinfos[2].maxsolutions = _nj2;
8902  vinfos[3].jointtype = 1;
8903  vinfos[3].foffset = j3;
8904  vinfos[3].indices[0] = _ij3[0];
8905  vinfos[3].indices[1] = _ij3[1];
8906  vinfos[3].maxsolutions = _nj3;
8907  vinfos[4].jointtype = 1;
8908  vinfos[4].foffset = j4;
8909  vinfos[4].indices[0] = _ij4[0];
8910  vinfos[4].indices[1] = _ij4[1];
8911  vinfos[4].maxsolutions = _nj4;
8912  vinfos[5].jointtype = 1;
8913  vinfos[5].foffset = j5;
8914  vinfos[5].indices[0] = _ij5[0];
8915  vinfos[5].indices[1] = _ij5[1];
8916  vinfos[5].maxsolutions = _nj5;
8917  vinfos[6].jointtype = 1;
8918  vinfos[6].foffset = j6;
8919  vinfos[6].indices[0] = _ij6[0];
8920  vinfos[6].indices[1] = _ij6[1];
8921  vinfos[6].maxsolutions = _nj6;
8922  std::vector<int> vfree(0);
8923  solutions.AddSolution(vinfos, vfree);
8924  }
8925  }
8926  }
8927  }
8928  }
8929  }
8930  } while (0);
8931  if (bgotonextstatement)
8932  {
8933  bool bgotonextstatement = true;
8934  do
8935  {
8936  evalcond[0] = ((-3.14159265358979) +
8937  (IKfmod(((3.14159265358979) +
8938  (IKabs(((-1.5707963267949) + j2)))),
8939  6.28318530717959)));
8940  if (IKabs(evalcond[0]) < 0.0000050000000000)
8941  {
8942  bgotonextstatement = false;
8943  {
8944  IkReal j0array[1], cj0array[1], sj0array[1];
8945  bool j0valid[1] = { false };
8946  _nj0 = 1;
8947  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
8948  IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
8949  IKabs(IKsqr(new_r00) + IKsqr(((-1.0) * new_r10)) - 1) <=
8950  IKFAST_SINCOS_THRESH)
8951  continue;
8952  j0array[0] = IKatan2(new_r00, ((-1.0) * new_r10));
8953  sj0array[0] = IKsin(j0array[0]);
8954  cj0array[0] = IKcos(j0array[0]);
8955  if (j0array[0] > IKPI)
8956  {
8957  j0array[0] -= IK2PI;
8958  }
8959  else if (j0array[0] < -IKPI)
8960  {
8961  j0array[0] += IK2PI;
8962  }
8963  j0valid[0] = true;
8964  for (int ij0 = 0; ij0 < 1; ++ij0)
8965  {
8966  if (!j0valid[ij0])
8967  {
8968  continue;
8969  }
8970  _ij0[0] = ij0;
8971  _ij0[1] = -1;
8972  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8973  {
8974  if (j0valid[iij0] &&
8975  IKabs(cj0array[ij0] - cj0array[iij0]) <
8976  IKFAST_SOLUTION_THRESH &&
8977  IKabs(sj0array[ij0] - sj0array[iij0]) <
8978  IKFAST_SOLUTION_THRESH)
8979  {
8980  j0valid[iij0] = false;
8981  _ij0[1] = iij0;
8982  break;
8983  }
8984  }
8985  j0 = j0array[ij0];
8986  cj0 = cj0array[ij0];
8987  sj0 = sj0array[ij0];
8988  {
8989  IkReal evalcond[8];
8990  IkReal x433 = IKcos(j0);
8991  IkReal x434 = IKsin(j0);
8992  IkReal x435 = ((1.0) * x434);
8993  evalcond[0] = (x433 + new_r10);
8994  evalcond[1] = ((((-1.0) * x435)) + new_r00);
8995  evalcond[2] = ((((-1.0) * x435)) + new_r11);
8996  evalcond[3] = ((((-1.0) * x433)) + new_r01);
8997  evalcond[4] = (((new_r00 * x433)) + ((new_r10 * x434)));
8998  evalcond[5] =
8999  ((((-1.0) * new_r01 * x435)) + ((new_r11 * x433)));
9000  evalcond[6] =
9001  ((-1.0) + ((new_r01 * x433)) + ((new_r11 * x434)));
9002  evalcond[7] = ((1.0) + (((-1.0) * new_r00 * x435)) +
9003  ((new_r10 * x433)));
9004  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9005  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9006  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9007  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9008  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9009  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9010  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9011  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9012  {
9013  continue;
9014  }
9015  }
9016 
9017  {
9018  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9019  vinfos[0].jointtype = 1;
9020  vinfos[0].foffset = j0;
9021  vinfos[0].indices[0] = _ij0[0];
9022  vinfos[0].indices[1] = _ij0[1];
9023  vinfos[0].maxsolutions = _nj0;
9024  vinfos[1].jointtype = 1;
9025  vinfos[1].foffset = j1;
9026  vinfos[1].indices[0] = _ij1[0];
9027  vinfos[1].indices[1] = _ij1[1];
9028  vinfos[1].maxsolutions = _nj1;
9029  vinfos[2].jointtype = 1;
9030  vinfos[2].foffset = j2;
9031  vinfos[2].indices[0] = _ij2[0];
9032  vinfos[2].indices[1] = _ij2[1];
9033  vinfos[2].maxsolutions = _nj2;
9034  vinfos[3].jointtype = 1;
9035  vinfos[3].foffset = j3;
9036  vinfos[3].indices[0] = _ij3[0];
9037  vinfos[3].indices[1] = _ij3[1];
9038  vinfos[3].maxsolutions = _nj3;
9039  vinfos[4].jointtype = 1;
9040  vinfos[4].foffset = j4;
9041  vinfos[4].indices[0] = _ij4[0];
9042  vinfos[4].indices[1] = _ij4[1];
9043  vinfos[4].maxsolutions = _nj4;
9044  vinfos[5].jointtype = 1;
9045  vinfos[5].foffset = j5;
9046  vinfos[5].indices[0] = _ij5[0];
9047  vinfos[5].indices[1] = _ij5[1];
9048  vinfos[5].maxsolutions = _nj5;
9049  vinfos[6].jointtype = 1;
9050  vinfos[6].foffset = j6;
9051  vinfos[6].indices[0] = _ij6[0];
9052  vinfos[6].indices[1] = _ij6[1];
9053  vinfos[6].maxsolutions = _nj6;
9054  std::vector<int> vfree(0);
9055  solutions.AddSolution(vinfos, vfree);
9056  }
9057  }
9058  }
9059  }
9060  } while (0);
9061  if (bgotonextstatement)
9062  {
9063  bool bgotonextstatement = true;
9064  do
9065  {
9066  evalcond[0] = ((-3.14159265358979) +
9067  (IKfmod(((3.14159265358979) +
9068  (IKabs(((1.5707963267949) + j2)))),
9069  6.28318530717959)));
9070  if (IKabs(evalcond[0]) < 0.0000050000000000)
9071  {
9072  bgotonextstatement = false;
9073  {
9074  IkReal j0array[1], cj0array[1], sj0array[1];
9075  bool j0valid[1] = { false };
9076  _nj0 = 1;
9077  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
9078  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
9079  IKabs(IKsqr(((-1.0) * new_r00)) +
9080  IKsqr(((-1.0) * new_r01)) - 1) <=
9081  IKFAST_SINCOS_THRESH)
9082  continue;
9083  j0array[0] =
9084  IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
9085  sj0array[0] = IKsin(j0array[0]);
9086  cj0array[0] = IKcos(j0array[0]);
9087  if (j0array[0] > IKPI)
9088  {
9089  j0array[0] -= IK2PI;
9090  }
9091  else if (j0array[0] < -IKPI)
9092  {
9093  j0array[0] += IK2PI;
9094  }
9095  j0valid[0] = true;
9096  for (int ij0 = 0; ij0 < 1; ++ij0)
9097  {
9098  if (!j0valid[ij0])
9099  {
9100  continue;
9101  }
9102  _ij0[0] = ij0;
9103  _ij0[1] = -1;
9104  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9105  {
9106  if (j0valid[iij0] &&
9107  IKabs(cj0array[ij0] - cj0array[iij0]) <
9108  IKFAST_SOLUTION_THRESH &&
9109  IKabs(sj0array[ij0] - sj0array[iij0]) <
9110  IKFAST_SOLUTION_THRESH)
9111  {
9112  j0valid[iij0] = false;
9113  _ij0[1] = iij0;
9114  break;
9115  }
9116  }
9117  j0 = j0array[ij0];
9118  cj0 = cj0array[ij0];
9119  sj0 = sj0array[ij0];
9120  {
9121  IkReal evalcond[8];
9122  IkReal x436 = IKsin(j0);
9123  IkReal x437 = IKcos(j0);
9124  IkReal x438 = ((1.0) * x436);
9125  evalcond[0] = (x436 + new_r00);
9126  evalcond[1] = (x436 + new_r11);
9127  evalcond[2] = (x437 + new_r01);
9128  evalcond[3] = ((((-1.0) * x437)) + new_r10);
9129  evalcond[4] =
9130  (((new_r00 * x437)) + ((new_r10 * x436)));
9131  evalcond[5] = ((((-1.0) * new_r01 * x438)) +
9132  ((new_r11 * x437)));
9133  evalcond[6] =
9134  ((1.0) + ((new_r01 * x437)) + ((new_r11 * x436)));
9135  evalcond[7] = ((-1.0) + (((-1.0) * new_r00 * x438)) +
9136  ((new_r10 * x437)));
9137  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9138  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9139  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9140  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9141  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9142  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9143  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9144  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9145  {
9146  continue;
9147  }
9148  }
9149 
9150  {
9151  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
9152  7);
9153  vinfos[0].jointtype = 1;
9154  vinfos[0].foffset = j0;
9155  vinfos[0].indices[0] = _ij0[0];
9156  vinfos[0].indices[1] = _ij0[1];
9157  vinfos[0].maxsolutions = _nj0;
9158  vinfos[1].jointtype = 1;
9159  vinfos[1].foffset = j1;
9160  vinfos[1].indices[0] = _ij1[0];
9161  vinfos[1].indices[1] = _ij1[1];
9162  vinfos[1].maxsolutions = _nj1;
9163  vinfos[2].jointtype = 1;
9164  vinfos[2].foffset = j2;
9165  vinfos[2].indices[0] = _ij2[0];
9166  vinfos[2].indices[1] = _ij2[1];
9167  vinfos[2].maxsolutions = _nj2;
9168  vinfos[3].jointtype = 1;
9169  vinfos[3].foffset = j3;
9170  vinfos[3].indices[0] = _ij3[0];
9171  vinfos[3].indices[1] = _ij3[1];
9172  vinfos[3].maxsolutions = _nj3;
9173  vinfos[4].jointtype = 1;
9174  vinfos[4].foffset = j4;
9175  vinfos[4].indices[0] = _ij4[0];
9176  vinfos[4].indices[1] = _ij4[1];
9177  vinfos[4].maxsolutions = _nj4;
9178  vinfos[5].jointtype = 1;
9179  vinfos[5].foffset = j5;
9180  vinfos[5].indices[0] = _ij5[0];
9181  vinfos[5].indices[1] = _ij5[1];
9182  vinfos[5].maxsolutions = _nj5;
9183  vinfos[6].jointtype = 1;
9184  vinfos[6].foffset = j6;
9185  vinfos[6].indices[0] = _ij6[0];
9186  vinfos[6].indices[1] = _ij6[1];
9187  vinfos[6].maxsolutions = _nj6;
9188  std::vector<int> vfree(0);
9189  solutions.AddSolution(vinfos, vfree);
9190  }
9191  }
9192  }
9193  }
9194  } while (0);
9195  if (bgotonextstatement)
9196  {
9197  bool bgotonextstatement = true;
9198  do
9199  {
9200  evalcond[0] = ((new_r10 * new_r10) + (new_r00 * new_r00));
9201  if (IKabs(evalcond[0]) < 0.0000050000000000)
9202  {
9203  bgotonextstatement = false;
9204  {
9205  IkReal j0eval[1];
9206  sj1 = 0;
9207  cj1 = 1.0;
9208  j1 = 0;
9209  new_r10 = 0;
9210  new_r00 = 0;
9211  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9212  if (IKabs(j0eval[0]) < 0.0000010000000000)
9213  {
9214  continue; // 3 cases reached
9215  }
9216  else
9217  {
9218  {
9219  IkReal j0array[2], cj0array[2], sj0array[2];
9220  bool j0valid[2] = { false };
9221  _nj0 = 2;
9222  CheckValue<IkReal> x440 =
9223  IKatan2WithCheck(IkReal(new_r01),
9224  IkReal(new_r11),
9225  IKFAST_ATAN2_MAGTHRESH);
9226  if (!x440.valid)
9227  {
9228  continue;
9229  }
9230  IkReal x439 = x440.value;
9231  j0array[0] = ((-1.0) * x439);
9232  sj0array[0] = IKsin(j0array[0]);
9233  cj0array[0] = IKcos(j0array[0]);
9234  j0array[1] =
9235  ((3.14159265358979) + (((-1.0) * x439)));
9236  sj0array[1] = IKsin(j0array[1]);
9237  cj0array[1] = IKcos(j0array[1]);
9238  if (j0array[0] > IKPI)
9239  {
9240  j0array[0] -= IK2PI;
9241  }
9242  else if (j0array[0] < -IKPI)
9243  {
9244  j0array[0] += IK2PI;
9245  }
9246  j0valid[0] = true;
9247  if (j0array[1] > IKPI)
9248  {
9249  j0array[1] -= IK2PI;
9250  }
9251  else if (j0array[1] < -IKPI)
9252  {
9253  j0array[1] += IK2PI;
9254  }
9255  j0valid[1] = true;
9256  for (int ij0 = 0; ij0 < 2; ++ij0)
9257  {
9258  if (!j0valid[ij0])
9259  {
9260  continue;
9261  }
9262  _ij0[0] = ij0;
9263  _ij0[1] = -1;
9264  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9265  {
9266  if (j0valid[iij0] &&
9267  IKabs(cj0array[ij0] - cj0array[iij0]) <
9268  IKFAST_SOLUTION_THRESH &&
9269  IKabs(sj0array[ij0] - sj0array[iij0]) <
9270  IKFAST_SOLUTION_THRESH)
9271  {
9272  j0valid[iij0] = false;
9273  _ij0[1] = iij0;
9274  break;
9275  }
9276  }
9277  j0 = j0array[ij0];
9278  cj0 = cj0array[ij0];
9279  sj0 = sj0array[ij0];
9280  {
9281  IkReal evalcond[1];
9282  evalcond[0] =
9283  (((new_r11 * (IKcos(j0)))) +
9284  (((-1.0) * new_r01 * (IKsin(j0)))));
9285  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
9286  {
9287  continue;
9288  }
9289  }
9290 
9291  {
9292  std::vector<IkSingleDOFSolutionBase<IkReal> >
9293  vinfos(7);
9294  vinfos[0].jointtype = 1;
9295  vinfos[0].foffset = j0;
9296  vinfos[0].indices[0] = _ij0[0];
9297  vinfos[0].indices[1] = _ij0[1];
9298  vinfos[0].maxsolutions = _nj0;
9299  vinfos[1].jointtype = 1;
9300  vinfos[1].foffset = j1;
9301  vinfos[1].indices[0] = _ij1[0];
9302  vinfos[1].indices[1] = _ij1[1];
9303  vinfos[1].maxsolutions = _nj1;
9304  vinfos[2].jointtype = 1;
9305  vinfos[2].foffset = j2;
9306  vinfos[2].indices[0] = _ij2[0];
9307  vinfos[2].indices[1] = _ij2[1];
9308  vinfos[2].maxsolutions = _nj2;
9309  vinfos[3].jointtype = 1;
9310  vinfos[3].foffset = j3;
9311  vinfos[3].indices[0] = _ij3[0];
9312  vinfos[3].indices[1] = _ij3[1];
9313  vinfos[3].maxsolutions = _nj3;
9314  vinfos[4].jointtype = 1;
9315  vinfos[4].foffset = j4;
9316  vinfos[4].indices[0] = _ij4[0];
9317  vinfos[4].indices[1] = _ij4[1];
9318  vinfos[4].maxsolutions = _nj4;
9319  vinfos[5].jointtype = 1;
9320  vinfos[5].foffset = j5;
9321  vinfos[5].indices[0] = _ij5[0];
9322  vinfos[5].indices[1] = _ij5[1];
9323  vinfos[5].maxsolutions = _nj5;
9324  vinfos[6].jointtype = 1;
9325  vinfos[6].foffset = j6;
9326  vinfos[6].indices[0] = _ij6[0];
9327  vinfos[6].indices[1] = _ij6[1];
9328  vinfos[6].maxsolutions = _nj6;
9329  std::vector<int> vfree(0);
9330  solutions.AddSolution(vinfos, vfree);
9331  }
9332  }
9333  }
9334  }
9335  }
9336  }
9337  } while (0);
9338  if (bgotonextstatement)
9339  {
9340  bool bgotonextstatement = true;
9341  do
9342  {
9343  evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
9344  if (IKabs(evalcond[0]) < 0.0000050000000000)
9345  {
9346  bgotonextstatement = false;
9347  {
9348  IkReal j0eval[1];
9349  sj1 = 0;
9350  cj1 = 1.0;
9351  j1 = 0;
9352  new_r00 = 0;
9353  new_r10 = 0;
9354  new_r21 = 0;
9355  new_r22 = 0;
9356  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9357  if (IKabs(j0eval[0]) < 0.0000010000000000)
9358  {
9359  continue; // no branches [j0]
9360  }
9361  else
9362  {
9363  {
9364  IkReal j0array[2], cj0array[2], sj0array[2];
9365  bool j0valid[2] = { false };
9366  _nj0 = 2;
9367  CheckValue<IkReal> x442 =
9368  IKatan2WithCheck(IkReal(new_r01),
9369  IkReal(new_r11),
9370  IKFAST_ATAN2_MAGTHRESH);
9371  if (!x442.valid)
9372  {
9373  continue;
9374  }
9375  IkReal x441 = x442.value;
9376  j0array[0] = ((-1.0) * x441);
9377  sj0array[0] = IKsin(j0array[0]);
9378  cj0array[0] = IKcos(j0array[0]);
9379  j0array[1] =
9380  ((3.14159265358979) + (((-1.0) * x441)));
9381  sj0array[1] = IKsin(j0array[1]);
9382  cj0array[1] = IKcos(j0array[1]);
9383  if (j0array[0] > IKPI)
9384  {
9385  j0array[0] -= IK2PI;
9386  }
9387  else if (j0array[0] < -IKPI)
9388  {
9389  j0array[0] += IK2PI;
9390  }
9391  j0valid[0] = true;
9392  if (j0array[1] > IKPI)
9393  {
9394  j0array[1] -= IK2PI;
9395  }
9396  else if (j0array[1] < -IKPI)
9397  {
9398  j0array[1] += IK2PI;
9399  }
9400  j0valid[1] = true;
9401  for (int ij0 = 0; ij0 < 2; ++ij0)
9402  {
9403  if (!j0valid[ij0])
9404  {
9405  continue;
9406  }
9407  _ij0[0] = ij0;
9408  _ij0[1] = -1;
9409  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9410  {
9411  if (j0valid[iij0] &&
9412  IKabs(cj0array[ij0] - cj0array[iij0]) <
9413  IKFAST_SOLUTION_THRESH &&
9414  IKabs(sj0array[ij0] - sj0array[iij0]) <
9415  IKFAST_SOLUTION_THRESH)
9416  {
9417  j0valid[iij0] = false;
9418  _ij0[1] = iij0;
9419  break;
9420  }
9421  }
9422  j0 = j0array[ij0];
9423  cj0 = cj0array[ij0];
9424  sj0 = sj0array[ij0];
9425  {
9426  IkReal evalcond[1];
9427  evalcond[0] =
9428  (((new_r11 * (IKcos(j0)))) +
9429  (((-1.0) * new_r01 * (IKsin(j0)))));
9430  if (IKabs(evalcond[0]) >
9431  IKFAST_EVALCOND_THRESH)
9432  {
9433  continue;
9434  }
9435  }
9436 
9437  {
9438  std::vector<IkSingleDOFSolutionBase<IkReal> >
9439  vinfos(7);
9440  vinfos[0].jointtype = 1;
9441  vinfos[0].foffset = j0;
9442  vinfos[0].indices[0] = _ij0[0];
9443  vinfos[0].indices[1] = _ij0[1];
9444  vinfos[0].maxsolutions = _nj0;
9445  vinfos[1].jointtype = 1;
9446  vinfos[1].foffset = j1;
9447  vinfos[1].indices[0] = _ij1[0];
9448  vinfos[1].indices[1] = _ij1[1];
9449  vinfos[1].maxsolutions = _nj1;
9450  vinfos[2].jointtype = 1;
9451  vinfos[2].foffset = j2;
9452  vinfos[2].indices[0] = _ij2[0];
9453  vinfos[2].indices[1] = _ij2[1];
9454  vinfos[2].maxsolutions = _nj2;
9455  vinfos[3].jointtype = 1;
9456  vinfos[3].foffset = j3;
9457  vinfos[3].indices[0] = _ij3[0];
9458  vinfos[3].indices[1] = _ij3[1];
9459  vinfos[3].maxsolutions = _nj3;
9460  vinfos[4].jointtype = 1;
9461  vinfos[4].foffset = j4;
9462  vinfos[4].indices[0] = _ij4[0];
9463  vinfos[4].indices[1] = _ij4[1];
9464  vinfos[4].maxsolutions = _nj4;
9465  vinfos[5].jointtype = 1;
9466  vinfos[5].foffset = j5;
9467  vinfos[5].indices[0] = _ij5[0];
9468  vinfos[5].indices[1] = _ij5[1];
9469  vinfos[5].maxsolutions = _nj5;
9470  vinfos[6].jointtype = 1;
9471  vinfos[6].foffset = j6;
9472  vinfos[6].indices[0] = _ij6[0];
9473  vinfos[6].indices[1] = _ij6[1];
9474  vinfos[6].maxsolutions = _nj6;
9475  std::vector<int> vfree(0);
9476  solutions.AddSolution(vinfos, vfree);
9477  }
9478  }
9479  }
9480  }
9481  }
9482  }
9483  } while (0);
9484  if (bgotonextstatement)
9485  {
9486  bool bgotonextstatement = true;
9487  do
9488  {
9489  if (1)
9490  {
9491  bgotonextstatement = false;
9492  continue; // branch miss [j0]
9493  }
9494  } while (0);
9495  if (bgotonextstatement)
9496  {
9497  }
9498  }
9499  }
9500  }
9501  }
9502  }
9503  }
9504  }
9505  }
9506  else
9507  {
9508  {
9509  IkReal j0array[1], cj0array[1], sj0array[1];
9510  bool j0valid[1] = { false };
9511  _nj0 = 1;
9512  IkReal x443 = ((1.0) * new_r10);
9513  CheckValue<IkReal> x444 = IKatan2WithCheck(
9514  IkReal((((cj2 * sj2)) + (((-1.0) * new_r00 * x443)))),
9515  IkReal(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10))),
9516  IKFAST_ATAN2_MAGTHRESH);
9517  if (!x444.valid)
9518  {
9519  continue;
9520  }
9521  CheckValue<IkReal> x445 = IKPowWithIntegerCheck(
9522  IKsign(((((-1.0) * sj2 * x443)) + ((cj2 * new_r00)))), -1);
9523  if (!x445.valid)
9524  {
9525  continue;
9526  }
9527  j0array[0] = ((-1.5707963267949) + (x444.value) +
9528  (((1.5707963267949) * (x445.value))));
9529  sj0array[0] = IKsin(j0array[0]);
9530  cj0array[0] = IKcos(j0array[0]);
9531  if (j0array[0] > IKPI)
9532  {
9533  j0array[0] -= IK2PI;
9534  }
9535  else if (j0array[0] < -IKPI)
9536  {
9537  j0array[0] += IK2PI;
9538  }
9539  j0valid[0] = true;
9540  for (int ij0 = 0; ij0 < 1; ++ij0)
9541  {
9542  if (!j0valid[ij0])
9543  {
9544  continue;
9545  }
9546  _ij0[0] = ij0;
9547  _ij0[1] = -1;
9548  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9549  {
9550  if (j0valid[iij0] &&
9551  IKabs(cj0array[ij0] - cj0array[iij0]) <
9552  IKFAST_SOLUTION_THRESH &&
9553  IKabs(sj0array[ij0] - sj0array[iij0]) <
9554  IKFAST_SOLUTION_THRESH)
9555  {
9556  j0valid[iij0] = false;
9557  _ij0[1] = iij0;
9558  break;
9559  }
9560  }
9561  j0 = j0array[ij0];
9562  cj0 = cj0array[ij0];
9563  sj0 = sj0array[ij0];
9564  {
9565  IkReal evalcond[8];
9566  IkReal x446 = IKcos(j0);
9567  IkReal x447 = IKsin(j0);
9568  IkReal x448 = ((1.0) * sj2);
9569  IkReal x449 = (cj2 * x446);
9570  IkReal x450 = ((1.0) * x447);
9571  IkReal x451 = (x447 * x448);
9572  evalcond[0] = (((new_r00 * x446)) + cj2 + ((new_r10 * x447)));
9573  evalcond[1] = (((cj2 * x447)) + ((sj2 * x446)) + new_r10);
9574  evalcond[2] =
9575  (sj2 + (((-1.0) * new_r00 * x450)) + ((new_r10 * x446)));
9576  evalcond[3] =
9577  (cj2 + (((-1.0) * new_r01 * x450)) + ((new_r11 * x446)));
9578  evalcond[4] = ((((-1.0) * x451)) + x449 + new_r00);
9579  evalcond[5] = ((((-1.0) * x451)) + x449 + new_r11);
9580  evalcond[6] =
9581  ((((-1.0) * x448)) + ((new_r01 * x446)) + ((new_r11 * x447)));
9582  evalcond[7] = (new_r01 + (((-1.0) * cj2 * x450)) +
9583  (((-1.0) * x446 * x448)));
9584  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9585  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9586  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9587  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9588  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9589  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9590  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9591  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9592  {
9593  continue;
9594  }
9595  }
9596 
9597  {
9598  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9599  vinfos[0].jointtype = 1;
9600  vinfos[0].foffset = j0;
9601  vinfos[0].indices[0] = _ij0[0];
9602  vinfos[0].indices[1] = _ij0[1];
9603  vinfos[0].maxsolutions = _nj0;
9604  vinfos[1].jointtype = 1;
9605  vinfos[1].foffset = j1;
9606  vinfos[1].indices[0] = _ij1[0];
9607  vinfos[1].indices[1] = _ij1[1];
9608  vinfos[1].maxsolutions = _nj1;
9609  vinfos[2].jointtype = 1;
9610  vinfos[2].foffset = j2;
9611  vinfos[2].indices[0] = _ij2[0];
9612  vinfos[2].indices[1] = _ij2[1];
9613  vinfos[2].maxsolutions = _nj2;
9614  vinfos[3].jointtype = 1;
9615  vinfos[3].foffset = j3;
9616  vinfos[3].indices[0] = _ij3[0];
9617  vinfos[3].indices[1] = _ij3[1];
9618  vinfos[3].maxsolutions = _nj3;
9619  vinfos[4].jointtype = 1;
9620  vinfos[4].foffset = j4;
9621  vinfos[4].indices[0] = _ij4[0];
9622  vinfos[4].indices[1] = _ij4[1];
9623  vinfos[4].maxsolutions = _nj4;
9624  vinfos[5].jointtype = 1;
9625  vinfos[5].foffset = j5;
9626  vinfos[5].indices[0] = _ij5[0];
9627  vinfos[5].indices[1] = _ij5[1];
9628  vinfos[5].maxsolutions = _nj5;
9629  vinfos[6].jointtype = 1;
9630  vinfos[6].foffset = j6;
9631  vinfos[6].indices[0] = _ij6[0];
9632  vinfos[6].indices[1] = _ij6[1];
9633  vinfos[6].maxsolutions = _nj6;
9634  std::vector<int> vfree(0);
9635  solutions.AddSolution(vinfos, vfree);
9636  }
9637  }
9638  }
9639  }
9640  }
9641  }
9642  else
9643  {
9644  {
9645  IkReal j0array[1], cj0array[1], sj0array[1];
9646  bool j0valid[1] = { false };
9647  _nj0 = 1;
9648  IkReal x452 = ((1.0) * cj2);
9649  CheckValue<IkReal> x453 = IKatan2WithCheck(
9650  IkReal(((((-1.0) * new_r11 * x452)) + ((cj2 * new_r00)))),
9651  IkReal(((((-1.0) * new_r10 * x452)) + (((-1.0) * new_r01 * x452)))),
9652  IKFAST_ATAN2_MAGTHRESH);
9653  if (!x453.valid)
9654  {
9655  continue;
9656  }
9657  CheckValue<IkReal> x454 = IKPowWithIntegerCheck(
9658  IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
9659  if (!x454.valid)
9660  {
9661  continue;
9662  }
9663  j0array[0] = ((-1.5707963267949) + (x453.value) +
9664  (((1.5707963267949) * (x454.value))));
9665  sj0array[0] = IKsin(j0array[0]);
9666  cj0array[0] = IKcos(j0array[0]);
9667  if (j0array[0] > IKPI)
9668  {
9669  j0array[0] -= IK2PI;
9670  }
9671  else if (j0array[0] < -IKPI)
9672  {
9673  j0array[0] += IK2PI;
9674  }
9675  j0valid[0] = true;
9676  for (int ij0 = 0; ij0 < 1; ++ij0)
9677  {
9678  if (!j0valid[ij0])
9679  {
9680  continue;
9681  }
9682  _ij0[0] = ij0;
9683  _ij0[1] = -1;
9684  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9685  {
9686  if (j0valid[iij0] &&
9687  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9688  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9689  {
9690  j0valid[iij0] = false;
9691  _ij0[1] = iij0;
9692  break;
9693  }
9694  }
9695  j0 = j0array[ij0];
9696  cj0 = cj0array[ij0];
9697  sj0 = sj0array[ij0];
9698  {
9699  IkReal evalcond[8];
9700  IkReal x455 = IKcos(j0);
9701  IkReal x456 = IKsin(j0);
9702  IkReal x457 = ((1.0) * sj2);
9703  IkReal x458 = (cj2 * x455);
9704  IkReal x459 = ((1.0) * x456);
9705  IkReal x460 = (x456 * x457);
9706  evalcond[0] = (((new_r10 * x456)) + cj2 + ((new_r00 * x455)));
9707  evalcond[1] = (((sj2 * x455)) + ((cj2 * x456)) + new_r10);
9708  evalcond[2] =
9709  (((new_r10 * x455)) + sj2 + (((-1.0) * new_r00 * x459)));
9710  evalcond[3] =
9711  (((new_r11 * x455)) + cj2 + (((-1.0) * new_r01 * x459)));
9712  evalcond[4] = ((((-1.0) * x460)) + x458 + new_r00);
9713  evalcond[5] = ((((-1.0) * x460)) + x458 + new_r11);
9714  evalcond[6] =
9715  (((new_r11 * x456)) + ((new_r01 * x455)) + (((-1.0) * x457)));
9716  evalcond[7] =
9717  ((((-1.0) * x455 * x457)) + new_r01 + (((-1.0) * cj2 * x459)));
9718  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9719  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9720  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9721  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9722  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9723  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9724  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9725  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9726  {
9727  continue;
9728  }
9729  }
9730 
9731  {
9732  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9733  vinfos[0].jointtype = 1;
9734  vinfos[0].foffset = j0;
9735  vinfos[0].indices[0] = _ij0[0];
9736  vinfos[0].indices[1] = _ij0[1];
9737  vinfos[0].maxsolutions = _nj0;
9738  vinfos[1].jointtype = 1;
9739  vinfos[1].foffset = j1;
9740  vinfos[1].indices[0] = _ij1[0];
9741  vinfos[1].indices[1] = _ij1[1];
9742  vinfos[1].maxsolutions = _nj1;
9743  vinfos[2].jointtype = 1;
9744  vinfos[2].foffset = j2;
9745  vinfos[2].indices[0] = _ij2[0];
9746  vinfos[2].indices[1] = _ij2[1];
9747  vinfos[2].maxsolutions = _nj2;
9748  vinfos[3].jointtype = 1;
9749  vinfos[3].foffset = j3;
9750  vinfos[3].indices[0] = _ij3[0];
9751  vinfos[3].indices[1] = _ij3[1];
9752  vinfos[3].maxsolutions = _nj3;
9753  vinfos[4].jointtype = 1;
9754  vinfos[4].foffset = j4;
9755  vinfos[4].indices[0] = _ij4[0];
9756  vinfos[4].indices[1] = _ij4[1];
9757  vinfos[4].maxsolutions = _nj4;
9758  vinfos[5].jointtype = 1;
9759  vinfos[5].foffset = j5;
9760  vinfos[5].indices[0] = _ij5[0];
9761  vinfos[5].indices[1] = _ij5[1];
9762  vinfos[5].maxsolutions = _nj5;
9763  vinfos[6].jointtype = 1;
9764  vinfos[6].foffset = j6;
9765  vinfos[6].indices[0] = _ij6[0];
9766  vinfos[6].indices[1] = _ij6[1];
9767  vinfos[6].maxsolutions = _nj6;
9768  std::vector<int> vfree(0);
9769  solutions.AddSolution(vinfos, vfree);
9770  }
9771  }
9772  }
9773  }
9774  }
9775  }
9776  else
9777  {
9778  {
9779  IkReal j0array[1], cj0array[1], sj0array[1];
9780  bool j0valid[1] = { false };
9781  _nj0 = 1;
9782  IkReal x461 = ((1.0) * cj2);
9783  CheckValue<IkReal> x462 = IKPowWithIntegerCheck(
9784  IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))), -1);
9785  if (!x462.valid)
9786  {
9787  continue;
9788  }
9789  CheckValue<IkReal> x463 = IKatan2WithCheck(
9790  IkReal(((((-1.0) * new_r10 * x461)) + ((new_r00 * sj2)))),
9791  IkReal(((((-1.0) * new_r00 * x461)) + (((-1.0) * new_r10 * sj2)))),
9792  IKFAST_ATAN2_MAGTHRESH);
9793  if (!x463.valid)
9794  {
9795  continue;
9796  }
9797  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x462.value))) +
9798  (x463.value));
9799  sj0array[0] = IKsin(j0array[0]);
9800  cj0array[0] = IKcos(j0array[0]);
9801  if (j0array[0] > IKPI)
9802  {
9803  j0array[0] -= IK2PI;
9804  }
9805  else if (j0array[0] < -IKPI)
9806  {
9807  j0array[0] += IK2PI;
9808  }
9809  j0valid[0] = true;
9810  for (int ij0 = 0; ij0 < 1; ++ij0)
9811  {
9812  if (!j0valid[ij0])
9813  {
9814  continue;
9815  }
9816  _ij0[0] = ij0;
9817  _ij0[1] = -1;
9818  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9819  {
9820  if (j0valid[iij0] &&
9821  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9822  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9823  {
9824  j0valid[iij0] = false;
9825  _ij0[1] = iij0;
9826  break;
9827  }
9828  }
9829  j0 = j0array[ij0];
9830  cj0 = cj0array[ij0];
9831  sj0 = sj0array[ij0];
9832  {
9833  IkReal evalcond[8];
9834  IkReal x464 = IKcos(j0);
9835  IkReal x465 = IKsin(j0);
9836  IkReal x466 = ((1.0) * sj2);
9837  IkReal x467 = (cj2 * x464);
9838  IkReal x468 = ((1.0) * x465);
9839  IkReal x469 = (x465 * x466);
9840  evalcond[0] = (((new_r10 * x465)) + cj2 + ((new_r00 * x464)));
9841  evalcond[1] = (((cj2 * x465)) + new_r10 + ((sj2 * x464)));
9842  evalcond[2] = (sj2 + ((new_r10 * x464)) + (((-1.0) * new_r00 * x468)));
9843  evalcond[3] = (((new_r11 * x464)) + cj2 + (((-1.0) * new_r01 * x468)));
9844  evalcond[4] = ((((-1.0) * x469)) + x467 + new_r00);
9845  evalcond[5] = ((((-1.0) * x469)) + x467 + new_r11);
9846  evalcond[6] =
9847  (((new_r11 * x465)) + ((new_r01 * x464)) + (((-1.0) * x466)));
9848  evalcond[7] =
9849  ((((-1.0) * x464 * x466)) + new_r01 + (((-1.0) * cj2 * x468)));
9850  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9851  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9852  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9853  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9854  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9855  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9856  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9857  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9858  {
9859  continue;
9860  }
9861  }
9862 
9863  {
9864  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9865  vinfos[0].jointtype = 1;
9866  vinfos[0].foffset = j0;
9867  vinfos[0].indices[0] = _ij0[0];
9868  vinfos[0].indices[1] = _ij0[1];
9869  vinfos[0].maxsolutions = _nj0;
9870  vinfos[1].jointtype = 1;
9871  vinfos[1].foffset = j1;
9872  vinfos[1].indices[0] = _ij1[0];
9873  vinfos[1].indices[1] = _ij1[1];
9874  vinfos[1].maxsolutions = _nj1;
9875  vinfos[2].jointtype = 1;
9876  vinfos[2].foffset = j2;
9877  vinfos[2].indices[0] = _ij2[0];
9878  vinfos[2].indices[1] = _ij2[1];
9879  vinfos[2].maxsolutions = _nj2;
9880  vinfos[3].jointtype = 1;
9881  vinfos[3].foffset = j3;
9882  vinfos[3].indices[0] = _ij3[0];
9883  vinfos[3].indices[1] = _ij3[1];
9884  vinfos[3].maxsolutions = _nj3;
9885  vinfos[4].jointtype = 1;
9886  vinfos[4].foffset = j4;
9887  vinfos[4].indices[0] = _ij4[0];
9888  vinfos[4].indices[1] = _ij4[1];
9889  vinfos[4].maxsolutions = _nj4;
9890  vinfos[5].jointtype = 1;
9891  vinfos[5].foffset = j5;
9892  vinfos[5].indices[0] = _ij5[0];
9893  vinfos[5].indices[1] = _ij5[1];
9894  vinfos[5].maxsolutions = _nj5;
9895  vinfos[6].jointtype = 1;
9896  vinfos[6].foffset = j6;
9897  vinfos[6].indices[0] = _ij6[0];
9898  vinfos[6].indices[1] = _ij6[1];
9899  vinfos[6].maxsolutions = _nj6;
9900  std::vector<int> vfree(0);
9901  solutions.AddSolution(vinfos, vfree);
9902  }
9903  }
9904  }
9905  }
9906  }
9907  }
9908  } while (0);
9909  if (bgotonextstatement)
9910  {
9911  bool bgotonextstatement = true;
9912  do
9913  {
9914  evalcond[0] = ((-3.14159265358979) +
9915  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
9916  6.28318530717959)));
9917  evalcond[1] = new_r21;
9918  evalcond[2] = new_r02;
9919  evalcond[3] = new_r12;
9920  evalcond[4] = new_r20;
9921  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
9922  IKabs(evalcond[1]) < 0.0000050000000000 &&
9923  IKabs(evalcond[2]) < 0.0000050000000000 &&
9924  IKabs(evalcond[3]) < 0.0000050000000000 &&
9925  IKabs(evalcond[4]) < 0.0000050000000000)
9926  {
9927  bgotonextstatement = false;
9928  {
9929  IkReal j0eval[3];
9930  sj1 = 0;
9931  cj1 = -1.0;
9932  j1 = 3.14159265358979;
9933  IkReal x470 = ((1.0) * sj2);
9934  IkReal x471 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
9935  j0eval[0] = x471;
9936  j0eval[1] =
9937  ((IKabs(((((-1.0) * new_r00 * x470)) + (((-1.0) * new_r11 * x470))))) +
9938  (IKabs((((new_r01 * sj2)) + (((-1.0) * new_r10 * x470))))));
9939  j0eval[2] = IKsign(x471);
9940  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9941  IKabs(j0eval[1]) < 0.0000010000000000 ||
9942  IKabs(j0eval[2]) < 0.0000010000000000)
9943  {
9944  {
9945  IkReal j0eval[3];
9946  sj1 = 0;
9947  cj1 = -1.0;
9948  j1 = 3.14159265358979;
9949  IkReal x472 = ((1.0) * new_r11);
9950  IkReal x473 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9951  j0eval[0] = x473;
9952  j0eval[1] =
9953  ((IKabs(((((-1.0) * sj2 * x472)) + ((cj2 * new_r01))))) +
9954  (IKabs(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x472))))));
9955  j0eval[2] = IKsign(x473);
9956  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9957  IKabs(j0eval[1]) < 0.0000010000000000 ||
9958  IKabs(j0eval[2]) < 0.0000010000000000)
9959  {
9960  {
9961  IkReal j0eval[3];
9962  sj1 = 0;
9963  cj1 = -1.0;
9964  j1 = 3.14159265358979;
9965  IkReal x474 = (((new_r11 * sj2)) + ((cj2 * new_r01)));
9966  j0eval[0] = x474;
9967  j0eval[1] = IKsign(x474);
9968  j0eval[2] = ((IKabs(((((-1.0) * cj2 * sj2)) +
9969  (((-1.0) * new_r10 * new_r11))))) +
9970  (IKabs(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10))))));
9971  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9972  IKabs(j0eval[1]) < 0.0000010000000000 ||
9973  IKabs(j0eval[2]) < 0.0000010000000000)
9974  {
9975  {
9976  IkReal evalcond[1];
9977  bool bgotonextstatement = true;
9978  do
9979  {
9980  IkReal x476 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9981  if (IKabs(x476) == 0)
9982  {
9983  continue;
9984  }
9985  IkReal x475 = pow(x476, -0.5);
9986  CheckValue<IkReal> x477 = IKatan2WithCheck(
9987  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9988  if (!x477.valid)
9989  {
9990  continue;
9991  }
9992  IkReal gconst6 = ((-1.0) * (x477.value));
9993  IkReal gconst7 = ((-1.0) * new_r01 * x475);
9994  IkReal gconst8 = (new_r11 * x475);
9995  CheckValue<IkReal> x478 = IKatan2WithCheck(
9996  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9997  if (!x478.valid)
9998  {
9999  continue;
10000  }
10001  evalcond[0] =
10002  ((-3.14159265358979) +
10003  (IKfmod(((3.14159265358979) + (IKabs(((x478.value) + j2)))),
10004  6.28318530717959)));
10005  if (IKabs(evalcond[0]) < 0.0000050000000000)
10006  {
10007  bgotonextstatement = false;
10008  {
10009  IkReal j0eval[3];
10010  CheckValue<IkReal> x481 = IKatan2WithCheck(
10011  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
10012  if (!x481.valid)
10013  {
10014  continue;
10015  }
10016  IkReal x479 = ((-1.0) * (x481.value));
10017  IkReal x480 = x475;
10018  sj1 = 0;
10019  cj1 = -1.0;
10020  j1 = 3.14159265358979;
10021  sj2 = gconst7;
10022  cj2 = gconst8;
10023  j2 = x479;
10024  IkReal gconst6 = x479;
10025  IkReal gconst7 = ((-1.0) * new_r01 * x480);
10026  IkReal gconst8 = (new_r11 * x480);
10027  IkReal x482 = new_r01 * new_r01;
10028  IkReal x483 = (new_r00 * new_r01);
10029  IkReal x484 = (((new_r10 * new_r11)) + x483);
10030  IkReal x485 = x475;
10031  IkReal x486 = (new_r01 * x485);
10032  j0eval[0] = x484;
10033  j0eval[1] = IKsign(x484);
10034  j0eval[2] = ((IKabs((((new_r10 * x486)) +
10035  (((-1.0) * x482 * x485))))) +
10036  (IKabs((((x483 * x485)) + ((new_r11 * x486))))));
10037  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10038  IKabs(j0eval[1]) < 0.0000010000000000 ||
10039  IKabs(j0eval[2]) < 0.0000010000000000)
10040  {
10041  {
10042  IkReal j0eval[2];
10043  CheckValue<IkReal> x489 =
10044  IKatan2WithCheck(IkReal(new_r01),
10045  IkReal(new_r11),
10046  IKFAST_ATAN2_MAGTHRESH);
10047  if (!x489.valid)
10048  {
10049  continue;
10050  }
10051  IkReal x487 = ((-1.0) * (x489.value));
10052  IkReal x488 = x475;
10053  sj1 = 0;
10054  cj1 = -1.0;
10055  j1 = 3.14159265358979;
10056  sj2 = gconst7;
10057  cj2 = gconst8;
10058  j2 = x487;
10059  IkReal gconst6 = x487;
10060  IkReal gconst7 = ((-1.0) * new_r01 * x488);
10061  IkReal gconst8 = (new_r11 * x488);
10062  IkReal x490 = ((new_r01 * new_r01) + (new_r11 * new_r11));
10063  j0eval[0] = x490;
10064  j0eval[1] = IKsign(x490);
10065  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10066  IKabs(j0eval[1]) < 0.0000010000000000)
10067  {
10068  {
10069  IkReal j0eval[1];
10070  CheckValue<IkReal> x493 =
10071  IKatan2WithCheck(IkReal(new_r01),
10072  IkReal(new_r11),
10073  IKFAST_ATAN2_MAGTHRESH);
10074  if (!x493.valid)
10075  {
10076  continue;
10077  }
10078  IkReal x491 = ((-1.0) * (x493.value));
10079  IkReal x492 = x475;
10080  sj1 = 0;
10081  cj1 = -1.0;
10082  j1 = 3.14159265358979;
10083  sj2 = gconst7;
10084  cj2 = gconst8;
10085  j2 = x491;
10086  IkReal gconst6 = x491;
10087  IkReal gconst7 = ((-1.0) * new_r01 * x492);
10088  IkReal gconst8 = (new_r11 * x492);
10089  IkReal x494 = new_r01 * new_r01;
10090  IkReal x495 = new_r11 * new_r11;
10091  IkReal x496 = ((1.0) * x494);
10092  CheckValue<IkReal> x502 =
10093  IKPowWithIntegerCheck((x494 + x495), -1);
10094  if (!x502.valid)
10095  {
10096  continue;
10097  }
10098  IkReal x497 = x502.value;
10099  CheckValue<IkReal> x503 = IKPowWithIntegerCheck(
10100  ((((-1.0) * x496)) + (((-1.0) * x495))), -1);
10101  if (!x503.valid)
10102  {
10103  continue;
10104  }
10105  IkReal x498 = x503.value;
10106  IkReal x499 = ((1.0) * x498);
10107  IkReal x500 = (new_r11 * x499);
10108  IkReal x501 = (new_r01 * x499);
10109  j0eval[0] =
10110  ((IKabs((((x494 * x495 * x497)) +
10111  (((-1.0) * x496 * x497)) +
10112  ((x497 * (x495 * x495)))))) +
10113  (IKabs(((((-1.0) * new_r01 * x500)) +
10114  (((-1.0) * x500 *
10115  (new_r01 * new_r01 * new_r01))) +
10116  (((-1.0) * new_r01 * x500 *
10117  (new_r11 * new_r11)))))));
10118  if (IKabs(j0eval[0]) < 0.0000010000000000)
10119  {
10120  {
10121  IkReal evalcond[3];
10122  bool bgotonextstatement = true;
10123  do
10124  {
10125  evalcond[0] =
10126  ((IKabs(new_r11)) + (IKabs(new_r00)));
10127  if (IKabs(evalcond[0]) < 0.0000050000000000)
10128  {
10129  bgotonextstatement = false;
10130  {
10131  IkReal j0eval[1];
10132  CheckValue<IkReal> x505 = IKatan2WithCheck(
10133  IkReal(new_r01),
10134  IkReal(0),
10135  IKFAST_ATAN2_MAGTHRESH);
10136  if (!x505.valid)
10137  {
10138  continue;
10139  }
10140  IkReal x504 = ((-1.0) * (x505.value));
10141  sj1 = 0;
10142  cj1 = -1.0;
10143  j1 = 3.14159265358979;
10144  sj2 = gconst7;
10145  cj2 = gconst8;
10146  j2 = x504;
10147  new_r11 = 0;
10148  new_r00 = 0;
10149  IkReal gconst6 = x504;
10150  IkReal x506 = new_r01 * new_r01;
10151  if (IKabs(x506) == 0)
10152  {
10153  continue;
10154  }
10155  IkReal gconst7 =
10156  ((-1.0) * new_r01 * (pow(x506, -0.5)));
10157  IkReal gconst8 = 0;
10158  j0eval[0] = new_r10;
10159  if (IKabs(j0eval[0]) < 0.0000010000000000)
10160  {
10161  {
10162  IkReal j0array[2], cj0array[2],
10163  sj0array[2];
10164  bool j0valid[2] = { false };
10165  _nj0 = 2;
10166  CheckValue<IkReal> x507 =
10167  IKPowWithIntegerCheck(gconst7, -1);
10168  if (!x507.valid)
10169  {
10170  continue;
10171  }
10172  cj0array[0] =
10173  ((-1.0) * new_r10 * (x507.value));
10174  if (cj0array[0] >=
10175  -1 - IKFAST_SINCOS_THRESH &&
10176  cj0array[0] <=
10177  1 + IKFAST_SINCOS_THRESH)
10178  {
10179  j0valid[0] = j0valid[1] = true;
10180  j0array[0] = IKacos(cj0array[0]);
10181  sj0array[0] = IKsin(j0array[0]);
10182  cj0array[1] = cj0array[0];
10183  j0array[1] = -j0array[0];
10184  sj0array[1] = -sj0array[0];
10185  }
10186  else if (isnan(cj0array[0]))
10187  {
10188  // probably any value will work
10189  j0valid[0] = true;
10190  cj0array[0] = 1;
10191  sj0array[0] = 0;
10192  j0array[0] = 0;
10193  }
10194  for (int ij0 = 0; ij0 < 2; ++ij0)
10195  {
10196  if (!j0valid[ij0])
10197  {
10198  continue;
10199  }
10200  _ij0[0] = ij0;
10201  _ij0[1] = -1;
10202  for (int iij0 = ij0 + 1; iij0 < 2;
10203  ++iij0)
10204  {
10205  if (j0valid[iij0] &&
10206  IKabs(cj0array[ij0] -
10207  cj0array[iij0]) <
10208  IKFAST_SOLUTION_THRESH &&
10209  IKabs(sj0array[ij0] -
10210  sj0array[iij0]) <
10211  IKFAST_SOLUTION_THRESH)
10212  {
10213  j0valid[iij0] = false;
10214  _ij0[1] = iij0;
10215  break;
10216  }
10217  }
10218  j0 = j0array[ij0];
10219  cj0 = cj0array[ij0];
10220  sj0 = sj0array[ij0];
10221  {
10222  IkReal evalcond[6];
10223  IkReal x508 = IKsin(j0);
10224  IkReal x509 = IKcos(j0);
10225  IkReal x510 = ((-1.0) * x508);
10226  evalcond[0] = (new_r10 * x508);
10227  evalcond[1] = (new_r01 * x510);
10228  evalcond[2] = (gconst7 * x510);
10229  evalcond[3] =
10230  (gconst7 + ((new_r10 * x509)));
10231  evalcond[4] =
10232  (gconst7 + ((new_r01 * x509)));
10233  evalcond[5] =
10234  (((gconst7 * x509)) + new_r01);
10235  if (IKabs(evalcond[0]) >
10236  IKFAST_EVALCOND_THRESH ||
10237  IKabs(evalcond[1]) >
10238  IKFAST_EVALCOND_THRESH ||
10239  IKabs(evalcond[2]) >
10240  IKFAST_EVALCOND_THRESH ||
10241  IKabs(evalcond[3]) >
10242  IKFAST_EVALCOND_THRESH ||
10243  IKabs(evalcond[4]) >
10244  IKFAST_EVALCOND_THRESH ||
10245  IKabs(evalcond[5]) >
10246  IKFAST_EVALCOND_THRESH)
10247  {
10248  continue;
10249  }
10250  }
10251 
10252  {
10253  std::vector<IkSingleDOFSolutionBase<
10254  IkReal> >
10255  vinfos(7);
10256  vinfos[0].jointtype = 1;
10257  vinfos[0].foffset = j0;
10258  vinfos[0].indices[0] = _ij0[0];
10259  vinfos[0].indices[1] = _ij0[1];
10260  vinfos[0].maxsolutions = _nj0;
10261  vinfos[1].jointtype = 1;
10262  vinfos[1].foffset = j1;
10263  vinfos[1].indices[0] = _ij1[0];
10264  vinfos[1].indices[1] = _ij1[1];
10265  vinfos[1].maxsolutions = _nj1;
10266  vinfos[2].jointtype = 1;
10267  vinfos[2].foffset = j2;
10268  vinfos[2].indices[0] = _ij2[0];
10269  vinfos[2].indices[1] = _ij2[1];
10270  vinfos[2].maxsolutions = _nj2;
10271  vinfos[3].jointtype = 1;
10272  vinfos[3].foffset = j3;
10273  vinfos[3].indices[0] = _ij3[0];
10274  vinfos[3].indices[1] = _ij3[1];
10275  vinfos[3].maxsolutions = _nj3;
10276  vinfos[4].jointtype = 1;
10277  vinfos[4].foffset = j4;
10278  vinfos[4].indices[0] = _ij4[0];
10279  vinfos[4].indices[1] = _ij4[1];
10280  vinfos[4].maxsolutions = _nj4;
10281  vinfos[5].jointtype = 1;
10282  vinfos[5].foffset = j5;
10283  vinfos[5].indices[0] = _ij5[0];
10284  vinfos[5].indices[1] = _ij5[1];
10285  vinfos[5].maxsolutions = _nj5;
10286  vinfos[6].jointtype = 1;
10287  vinfos[6].foffset = j6;
10288  vinfos[6].indices[0] = _ij6[0];
10289  vinfos[6].indices[1] = _ij6[1];
10290  vinfos[6].maxsolutions = _nj6;
10291  std::vector<int> vfree(0);
10292  solutions.AddSolution(vinfos,
10293  vfree);
10294  }
10295  }
10296  }
10297  }
10298  else
10299  {
10300  {
10301  IkReal j0array[2], cj0array[2],
10302  sj0array[2];
10303  bool j0valid[2] = { false };
10304  _nj0 = 2;
10305  CheckValue<IkReal> x511 =
10306  IKPowWithIntegerCheck(new_r10, -1);
10307  if (!x511.valid)
10308  {
10309  continue;
10310  }
10311  cj0array[0] =
10312  ((-1.0) * gconst7 * (x511.value));
10313  if (cj0array[0] >=
10314  -1 - IKFAST_SINCOS_THRESH &&
10315  cj0array[0] <=
10316  1 + IKFAST_SINCOS_THRESH)
10317  {
10318  j0valid[0] = j0valid[1] = true;
10319  j0array[0] = IKacos(cj0array[0]);
10320  sj0array[0] = IKsin(j0array[0]);
10321  cj0array[1] = cj0array[0];
10322  j0array[1] = -j0array[0];
10323  sj0array[1] = -sj0array[0];
10324  }
10325  else if (isnan(cj0array[0]))
10326  {
10327  // probably any value will work
10328  j0valid[0] = true;
10329  cj0array[0] = 1;
10330  sj0array[0] = 0;
10331  j0array[0] = 0;
10332  }
10333  for (int ij0 = 0; ij0 < 2; ++ij0)
10334  {
10335  if (!j0valid[ij0])
10336  {
10337  continue;
10338  }
10339  _ij0[0] = ij0;
10340  _ij0[1] = -1;
10341  for (int iij0 = ij0 + 1; iij0 < 2;
10342  ++iij0)
10343  {
10344  if (j0valid[iij0] &&
10345  IKabs(cj0array[ij0] -
10346  cj0array[iij0]) <
10347  IKFAST_SOLUTION_THRESH &&
10348  IKabs(sj0array[ij0] -
10349  sj0array[iij0]) <
10350  IKFAST_SOLUTION_THRESH)
10351  {
10352  j0valid[iij0] = false;
10353  _ij0[1] = iij0;
10354  break;
10355  }
10356  }
10357  j0 = j0array[ij0];
10358  cj0 = cj0array[ij0];
10359  sj0 = sj0array[ij0];
10360  {
10361  IkReal evalcond[6];
10362  IkReal x512 = IKsin(j0);
10363  IkReal x513 = IKcos(j0);
10364  IkReal x514 = (gconst7 * x513);
10365  IkReal x515 = ((-1.0) * x512);
10366  evalcond[0] = (new_r10 * x512);
10367  evalcond[1] = (new_r01 * x515);
10368  evalcond[2] = (gconst7 * x515);
10369  evalcond[3] = (x514 + new_r10);
10370  evalcond[4] =
10371  (((new_r01 * x513)) + gconst7);
10372  evalcond[5] = (x514 + new_r01);
10373  if (IKabs(evalcond[0]) >
10374  IKFAST_EVALCOND_THRESH ||
10375  IKabs(evalcond[1]) >
10376  IKFAST_EVALCOND_THRESH ||
10377  IKabs(evalcond[2]) >
10378  IKFAST_EVALCOND_THRESH ||
10379  IKabs(evalcond[3]) >
10380  IKFAST_EVALCOND_THRESH ||
10381  IKabs(evalcond[4]) >
10382  IKFAST_EVALCOND_THRESH ||
10383  IKabs(evalcond[5]) >
10384  IKFAST_EVALCOND_THRESH)
10385  {
10386  continue;
10387  }
10388  }
10389 
10390  {
10391  std::vector<IkSingleDOFSolutionBase<
10392  IkReal> >
10393  vinfos(7);
10394  vinfos[0].jointtype = 1;
10395  vinfos[0].foffset = j0;
10396  vinfos[0].indices[0] = _ij0[0];
10397  vinfos[0].indices[1] = _ij0[1];
10398  vinfos[0].maxsolutions = _nj0;
10399  vinfos[1].jointtype = 1;
10400  vinfos[1].foffset = j1;
10401  vinfos[1].indices[0] = _ij1[0];
10402  vinfos[1].indices[1] = _ij1[1];
10403  vinfos[1].maxsolutions = _nj1;
10404  vinfos[2].jointtype = 1;
10405  vinfos[2].foffset = j2;
10406  vinfos[2].indices[0] = _ij2[0];
10407  vinfos[2].indices[1] = _ij2[1];
10408  vinfos[2].maxsolutions = _nj2;
10409  vinfos[3].jointtype = 1;
10410  vinfos[3].foffset = j3;
10411  vinfos[3].indices[0] = _ij3[0];
10412  vinfos[3].indices[1] = _ij3[1];
10413  vinfos[3].maxsolutions = _nj3;
10414  vinfos[4].jointtype = 1;
10415  vinfos[4].foffset = j4;
10416  vinfos[4].indices[0] = _ij4[0];
10417  vinfos[4].indices[1] = _ij4[1];
10418  vinfos[4].maxsolutions = _nj4;
10419  vinfos[5].jointtype = 1;
10420  vinfos[5].foffset = j5;
10421  vinfos[5].indices[0] = _ij5[0];
10422  vinfos[5].indices[1] = _ij5[1];
10423  vinfos[5].maxsolutions = _nj5;
10424  vinfos[6].jointtype = 1;
10425  vinfos[6].foffset = j6;
10426  vinfos[6].indices[0] = _ij6[0];
10427  vinfos[6].indices[1] = _ij6[1];
10428  vinfos[6].maxsolutions = _nj6;
10429  std::vector<int> vfree(0);
10430  solutions.AddSolution(vinfos,
10431  vfree);
10432  }
10433  }
10434  }
10435  }
10436  }
10437  }
10438  } while (0);
10439  if (bgotonextstatement)
10440  {
10441  bool bgotonextstatement = true;
10442  do
10443  {
10444  evalcond[0] =
10445  ((IKabs(new_r10)) + (IKabs(new_r00)));
10446  evalcond[1] = gconst7;
10447  evalcond[2] = gconst8;
10448  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
10449  IKabs(evalcond[1]) < 0.0000050000000000 &&
10450  IKabs(evalcond[2]) < 0.0000050000000000)
10451  {
10452  bgotonextstatement = false;
10453  {
10454  IkReal j0eval[3];
10455  CheckValue<IkReal> x517 =
10456  IKatan2WithCheck(
10457  IkReal(new_r01),
10458  IkReal(new_r11),
10459  IKFAST_ATAN2_MAGTHRESH);
10460  if (!x517.valid)
10461  {
10462  continue;
10463  }
10464  IkReal x516 = ((-1.0) * (x517.value));
10465  sj1 = 0;
10466  cj1 = -1.0;
10467  j1 = 3.14159265358979;
10468  sj2 = gconst7;
10469  cj2 = gconst8;
10470  j2 = x516;
10471  new_r00 = 0;
10472  new_r10 = 0;
10473  new_r21 = 0;
10474  new_r22 = 0;
10475  IkReal gconst6 = x516;
10476  IkReal gconst7 = ((-1.0) * new_r01);
10477  IkReal gconst8 = new_r11;
10478  j0eval[0] = -1.0;
10479  j0eval[1] =
10480  ((IKabs((new_r01 * new_r11))) +
10481  (IKabs(((1.0) +
10482  (((-1.0) *
10483  (new_r01 * new_r01)))))));
10484  j0eval[2] = -1.0;
10485  if (IKabs(j0eval[0]) <
10486  0.0000010000000000 ||
10487  IKabs(j0eval[1]) <
10488  0.0000010000000000 ||
10489  IKabs(j0eval[2]) < 0.0000010000000000)
10490  {
10491  {
10492  IkReal j0eval[3];
10493  CheckValue<IkReal> x519 =
10494  IKatan2WithCheck(
10495  IkReal(new_r01),
10496  IkReal(new_r11),
10497  IKFAST_ATAN2_MAGTHRESH);
10498  if (!x519.valid)
10499  {
10500  continue;
10501  }
10502  IkReal x518 = ((-1.0) * (x519.value));
10503  sj1 = 0;
10504  cj1 = -1.0;
10505  j1 = 3.14159265358979;
10506  sj2 = gconst7;
10507  cj2 = gconst8;
10508  j2 = x518;
10509  new_r00 = 0;
10510  new_r10 = 0;
10511  new_r21 = 0;
10512  new_r22 = 0;
10513  IkReal gconst6 = x518;
10514  IkReal gconst7 = ((-1.0) * new_r01);
10515  IkReal gconst8 = new_r11;
10516  j0eval[0] = -1.0;
10517  j0eval[1] =
10518  ((IKabs((new_r01 * new_r11))) +
10519  (IKabs(
10520  ((1.0) +
10521  (((-1.0) *
10522  (new_r01 * new_r01)))))));
10523  j0eval[2] = -1.0;
10524  if (IKabs(j0eval[0]) <
10525  0.0000010000000000 ||
10526  IKabs(j0eval[1]) <
10527  0.0000010000000000 ||
10528  IKabs(j0eval[2]) <
10529  0.0000010000000000)
10530  {
10531  {
10532  IkReal j0eval[3];
10533  CheckValue<IkReal> x521 =
10534  IKatan2WithCheck(
10535  IkReal(new_r01),
10536  IkReal(new_r11),
10537  IKFAST_ATAN2_MAGTHRESH);
10538  if (!x521.valid)
10539  {
10540  continue;
10541  }
10542  IkReal x520 =
10543  ((-1.0) * (x521.value));
10544  sj1 = 0;
10545  cj1 = -1.0;
10546  j1 = 3.14159265358979;
10547  sj2 = gconst7;
10548  cj2 = gconst8;
10549  j2 = x520;
10550  new_r00 = 0;
10551  new_r10 = 0;
10552  new_r21 = 0;
10553  new_r22 = 0;
10554  IkReal gconst6 = x520;
10555  IkReal gconst7 =
10556  ((-1.0) * new_r01);
10557  IkReal gconst8 = new_r11;
10558  j0eval[0] = 1.0;
10559  j0eval[1] =
10560  ((((0.5) *
10561  (IKabs(
10562  ((-1.0) +
10563  (((2.0) *
10564  (new_r01 *
10565  new_r01)))))))) +
10566  (IKabs(
10567  (new_r01 * new_r11))));
10568  j0eval[2] = 1.0;
10569  if (IKabs(j0eval[0]) <
10570  0.0000010000000000 ||
10571  IKabs(j0eval[1]) <
10572  0.0000010000000000 ||
10573  IKabs(j0eval[2]) <
10574  0.0000010000000000)
10575  {
10576  continue; // 3 cases reached
10577  }
10578  else
10579  {
10580  {
10581  IkReal j0array[1],
10582  cj0array[1], sj0array[1];
10583  bool j0valid[1] = { false };
10584  _nj0 = 1;
10585  IkReal x522 =
10586  ((1.0) * new_r11);
10587  CheckValue<IkReal> x523 =
10588  IKatan2WithCheck(
10589  IkReal((((gconst8 *
10590  new_r01)) +
10591  (((-1.0) *
10592  gconst7 *
10593  x522)))),
10594  IkReal(((((-1.0) *
10595  gconst8 *
10596  x522)) +
10597  (((-1.0) *
10598  gconst7 *
10599  new_r01)))),
10600  IKFAST_ATAN2_MAGTHRESH);
10601  if (!x523.valid)
10602  {
10603  continue;
10604  }
10605  CheckValue<IkReal> x524 =
10606  IKPowWithIntegerCheck(
10607  IKsign(((new_r01 *
10608  new_r01) +
10609  (new_r11 *
10610  new_r11))),
10611  -1);
10612  if (!x524.valid)
10613  {
10614  continue;
10615  }
10616  j0array[0] =
10617  ((-1.5707963267949) +
10618  (x523.value) +
10619  (((1.5707963267949) *
10620  (x524.value))));
10621  sj0array[0] =
10622  IKsin(j0array[0]);
10623  cj0array[0] =
10624  IKcos(j0array[0]);
10625  if (j0array[0] > IKPI)
10626  {
10627  j0array[0] -= IK2PI;
10628  }
10629  else if (j0array[0] < -IKPI)
10630  {
10631  j0array[0] += IK2PI;
10632  }
10633  j0valid[0] = true;
10634  for (int ij0 = 0; ij0 < 1;
10635  ++ij0)
10636  {
10637  if (!j0valid[ij0])
10638  {
10639  continue;
10640  }
10641  _ij0[0] = ij0;
10642  _ij0[1] = -1;
10643  for (int iij0 = ij0 + 1;
10644  iij0 < 1;
10645  ++iij0)
10646  {
10647  if (j0valid[iij0] &&
10648  IKabs(
10649  cj0array[ij0] -
10650  cj0array[iij0]) <
10651  IKFAST_SOLUTION_THRESH &&
10652  IKabs(
10653  sj0array[ij0] -
10654  sj0array[iij0]) <
10655  IKFAST_SOLUTION_THRESH)
10656  {
10657  j0valid[iij0] = false;
10658  _ij0[1] = iij0;
10659  break;
10660  }
10661  }
10662  j0 = j0array[ij0];
10663  cj0 = cj0array[ij0];
10664  sj0 = sj0array[ij0];
10665  {
10666  IkReal evalcond[6];
10667  IkReal x525 = IKsin(j0);
10668  IkReal x526 = IKcos(j0);
10669  IkReal x527 =
10670  (gconst7 * x526);
10671  IkReal x528 =
10672  ((1.0) * x525);
10673  IkReal x529 =
10674  (gconst8 * x526);
10675  IkReal x530 =
10676  (gconst8 * x528);
10677  evalcond[0] =
10678  ((((-1.0) * x530)) +
10679  x527);
10680  evalcond[1] =
10681  (((new_r01 * x526)) +
10682  gconst7 +
10683  ((new_r11 * x525)));
10684  evalcond[2] =
10685  (((gconst7 * x525)) +
10686  x529 + new_r11);
10687  evalcond[3] =
10688  (gconst8 +
10689  ((new_r11 * x526)) +
10690  (((-1.0) * new_r01 *
10691  x528)));
10692  evalcond[4] =
10693  ((((-1.0) * x529)) +
10694  (((-1.0) * gconst7 *
10695  x528)));
10696  evalcond[5] =
10697  ((((-1.0) * x530)) +
10698  x527 + new_r01);
10699  if (IKabs(evalcond[0]) >
10700  IKFAST_EVALCOND_THRESH ||
10701  IKabs(evalcond[1]) >
10702  IKFAST_EVALCOND_THRESH ||
10703  IKabs(evalcond[2]) >
10704  IKFAST_EVALCOND_THRESH ||
10705  IKabs(evalcond[3]) >
10706  IKFAST_EVALCOND_THRESH ||
10707  IKabs(evalcond[4]) >
10708  IKFAST_EVALCOND_THRESH ||
10709  IKabs(evalcond[5]) >
10710  IKFAST_EVALCOND_THRESH)
10711  {
10712  continue;
10713  }
10714  }
10715 
10716  {
10717  std::vector<
10718  IkSingleDOFSolutionBase<
10719  IkReal> >
10720  vinfos(7);
10721  vinfos[0].jointtype = 1;
10722  vinfos[0].foffset = j0;
10723  vinfos[0].indices[0] =
10724  _ij0[0];
10725  vinfos[0].indices[1] =
10726  _ij0[1];
10727  vinfos[0].maxsolutions =
10728  _nj0;
10729  vinfos[1].jointtype = 1;
10730  vinfos[1].foffset = j1;
10731  vinfos[1].indices[0] =
10732  _ij1[0];
10733  vinfos[1].indices[1] =
10734  _ij1[1];
10735  vinfos[1].maxsolutions =
10736  _nj1;
10737  vinfos[2].jointtype = 1;
10738  vinfos[2].foffset = j2;
10739  vinfos[2].indices[0] =
10740  _ij2[0];
10741  vinfos[2].indices[1] =
10742  _ij2[1];
10743  vinfos[2].maxsolutions =
10744  _nj2;
10745  vinfos[3].jointtype = 1;
10746  vinfos[3].foffset = j3;
10747  vinfos[3].indices[0] =
10748  _ij3[0];
10749  vinfos[3].indices[1] =
10750  _ij3[1];
10751  vinfos[3].maxsolutions =
10752  _nj3;
10753  vinfos[4].jointtype = 1;
10754  vinfos[4].foffset = j4;
10755  vinfos[4].indices[0] =
10756  _ij4[0];
10757  vinfos[4].indices[1] =
10758  _ij4[1];
10759  vinfos[4].maxsolutions =
10760  _nj4;
10761  vinfos[5].jointtype = 1;
10762  vinfos[5].foffset = j5;
10763  vinfos[5].indices[0] =
10764  _ij5[0];
10765  vinfos[5].indices[1] =
10766  _ij5[1];
10767  vinfos[5].maxsolutions =
10768  _nj5;
10769  vinfos[6].jointtype = 1;
10770  vinfos[6].foffset = j6;
10771  vinfos[6].indices[0] =
10772  _ij6[0];
10773  vinfos[6].indices[1] =
10774  _ij6[1];
10775  vinfos[6].maxsolutions =
10776  _nj6;
10777  std::vector<int> vfree(0);
10778  solutions.AddSolution(
10779  vinfos, vfree);
10780  }
10781  }
10782  }
10783  }
10784  }
10785  }
10786  else
10787  {
10788  {
10789  IkReal j0array[1], cj0array[1],
10790  sj0array[1];
10791  bool j0valid[1] = { false };
10792  _nj0 = 1;
10793  CheckValue<IkReal> x531 =
10794  IKatan2WithCheck(
10795  IkReal(
10796  (gconst7 * new_r11)),
10797  IkReal(
10798  (gconst8 * new_r11)),
10799  IKFAST_ATAN2_MAGTHRESH);
10800  if (!x531.valid)
10801  {
10802  continue;
10803  }
10804  CheckValue<IkReal> x532 =
10805  IKPowWithIntegerCheck(
10806  IKsign(((((-1.0) *
10807  (gconst8 *
10808  gconst8))) +
10809  (((-1.0) *
10810  (gconst7 *
10811  gconst7))))),
10812  -1);
10813  if (!x532.valid)
10814  {
10815  continue;
10816  }
10817  j0array[0] =
10818  ((-1.5707963267949) +
10819  (x531.value) +
10820  (((1.5707963267949) *
10821  (x532.value))));
10822  sj0array[0] = IKsin(j0array[0]);
10823  cj0array[0] = IKcos(j0array[0]);
10824  if (j0array[0] > IKPI)
10825  {
10826  j0array[0] -= IK2PI;
10827  }
10828  else if (j0array[0] < -IKPI)
10829  {
10830  j0array[0] += IK2PI;
10831  }
10832  j0valid[0] = true;
10833  for (int ij0 = 0; ij0 < 1; ++ij0)
10834  {
10835  if (!j0valid[ij0])
10836  {
10837  continue;
10838  }
10839  _ij0[0] = ij0;
10840  _ij0[1] = -1;
10841  for (int iij0 = ij0 + 1;
10842  iij0 < 1;
10843  ++iij0)
10844  {
10845  if (j0valid[iij0] &&
10846  IKabs(cj0array[ij0] -
10847  cj0array[iij0]) <
10848  IKFAST_SOLUTION_THRESH &&
10849  IKabs(sj0array[ij0] -
10850  sj0array[iij0]) <
10851  IKFAST_SOLUTION_THRESH)
10852  {
10853  j0valid[iij0] = false;
10854  _ij0[1] = iij0;
10855  break;
10856  }
10857  }
10858  j0 = j0array[ij0];
10859  cj0 = cj0array[ij0];
10860  sj0 = sj0array[ij0];
10861  {
10862  IkReal evalcond[6];
10863  IkReal x533 = IKsin(j0);
10864  IkReal x534 = IKcos(j0);
10865  IkReal x535 =
10866  (gconst7 * x534);
10867  IkReal x536 = ((1.0) * x533);
10868  IkReal x537 =
10869  (gconst8 * x534);
10870  IkReal x538 =
10871  (gconst8 * x536);
10872  evalcond[0] =
10873  ((((-1.0) * x538)) +
10874  x535);
10875  evalcond[1] =
10876  (((new_r01 * x534)) +
10877  gconst7 +
10878  ((new_r11 * x533)));
10879  evalcond[2] =
10880  (((gconst7 * x533)) +
10881  x537 + new_r11);
10882  evalcond[3] =
10883  (gconst8 +
10884  ((new_r11 * x534)) +
10885  (((-1.0) * new_r01 *
10886  x536)));
10887  evalcond[4] =
10888  ((((-1.0) * x537)) +
10889  (((-1.0) * gconst7 *
10890  x536)));
10891  evalcond[5] =
10892  ((((-1.0) * x538)) +
10893  x535 + new_r01);
10894  if (IKabs(evalcond[0]) >
10895  IKFAST_EVALCOND_THRESH ||
10896  IKabs(evalcond[1]) >
10897  IKFAST_EVALCOND_THRESH ||
10898  IKabs(evalcond[2]) >
10899  IKFAST_EVALCOND_THRESH ||
10900  IKabs(evalcond[3]) >
10901  IKFAST_EVALCOND_THRESH ||
10902  IKabs(evalcond[4]) >
10903  IKFAST_EVALCOND_THRESH ||
10904  IKabs(evalcond[5]) >
10905  IKFAST_EVALCOND_THRESH)
10906  {
10907  continue;
10908  }
10909  }
10910 
10911  {
10912  std::vector<
10913  IkSingleDOFSolutionBase<
10914  IkReal> >
10915  vinfos(7);
10916  vinfos[0].jointtype = 1;
10917  vinfos[0].foffset = j0;
10918  vinfos[0].indices[0] =
10919  _ij0[0];
10920  vinfos[0].indices[1] =
10921  _ij0[1];
10922  vinfos[0].maxsolutions = _nj0;
10923  vinfos[1].jointtype = 1;
10924  vinfos[1].foffset = j1;
10925  vinfos[1].indices[0] =
10926  _ij1[0];
10927  vinfos[1].indices[1] =
10928  _ij1[1];
10929  vinfos[1].maxsolutions = _nj1;
10930  vinfos[2].jointtype = 1;
10931  vinfos[2].foffset = j2;
10932  vinfos[2].indices[0] =
10933  _ij2[0];
10934  vinfos[2].indices[1] =
10935  _ij2[1];
10936  vinfos[2].maxsolutions = _nj2;
10937  vinfos[3].jointtype = 1;
10938  vinfos[3].foffset = j3;
10939  vinfos[3].indices[0] =
10940  _ij3[0];
10941  vinfos[3].indices[1] =
10942  _ij3[1];
10943  vinfos[3].maxsolutions = _nj3;
10944  vinfos[4].jointtype = 1;
10945  vinfos[4].foffset = j4;
10946  vinfos[4].indices[0] =
10947  _ij4[0];
10948  vinfos[4].indices[1] =
10949  _ij4[1];
10950  vinfos[4].maxsolutions = _nj4;
10951  vinfos[5].jointtype = 1;
10952  vinfos[5].foffset = j5;
10953  vinfos[5].indices[0] =
10954  _ij5[0];
10955  vinfos[5].indices[1] =
10956  _ij5[1];
10957  vinfos[5].maxsolutions = _nj5;
10958  vinfos[6].jointtype = 1;
10959  vinfos[6].foffset = j6;
10960  vinfos[6].indices[0] =
10961  _ij6[0];
10962  vinfos[6].indices[1] =
10963  _ij6[1];
10964  vinfos[6].maxsolutions = _nj6;
10965  std::vector<int> vfree(0);
10966  solutions.AddSolution(vinfos,
10967  vfree);
10968  }
10969  }
10970  }
10971  }
10972  }
10973  }
10974  else
10975  {
10976  {
10977  IkReal j0array[1], cj0array[1],
10978  sj0array[1];
10979  bool j0valid[1] = { false };
10980  _nj0 = 1;
10981  CheckValue<IkReal> x539 =
10982  IKatan2WithCheck(
10983  IkReal((gconst7 * gconst8)),
10984  IkReal(gconst8 * gconst8),
10985  IKFAST_ATAN2_MAGTHRESH);
10986  if (!x539.valid)
10987  {
10988  continue;
10989  }
10990  CheckValue<IkReal> x540 =
10991  IKPowWithIntegerCheck(
10992  IKsign(
10993  ((((-1.0) * gconst8 *
10994  new_r11)) +
10995  ((gconst7 * new_r01)))),
10996  -1);
10997  if (!x540.valid)
10998  {
10999  continue;
11000  }
11001  j0array[0] = ((-1.5707963267949) +
11002  (x539.value) +
11003  (((1.5707963267949) *
11004  (x540.value))));
11005  sj0array[0] = IKsin(j0array[0]);
11006  cj0array[0] = IKcos(j0array[0]);
11007  if (j0array[0] > IKPI)
11008  {
11009  j0array[0] -= IK2PI;
11010  }
11011  else if (j0array[0] < -IKPI)
11012  {
11013  j0array[0] += IK2PI;
11014  }
11015  j0valid[0] = true;
11016  for (int ij0 = 0; ij0 < 1; ++ij0)
11017  {
11018  if (!j0valid[ij0])
11019  {
11020  continue;
11021  }
11022  _ij0[0] = ij0;
11023  _ij0[1] = -1;
11024  for (int iij0 = ij0 + 1; iij0 < 1;
11025  ++iij0)
11026  {
11027  if (j0valid[iij0] &&
11028  IKabs(cj0array[ij0] -
11029  cj0array[iij0]) <
11030  IKFAST_SOLUTION_THRESH &&
11031  IKabs(sj0array[ij0] -
11032  sj0array[iij0]) <
11033  IKFAST_SOLUTION_THRESH)
11034  {
11035  j0valid[iij0] = false;
11036  _ij0[1] = iij0;
11037  break;
11038  }
11039  }
11040  j0 = j0array[ij0];
11041  cj0 = cj0array[ij0];
11042  sj0 = sj0array[ij0];
11043  {
11044  IkReal evalcond[6];
11045  IkReal x541 = IKsin(j0);
11046  IkReal x542 = IKcos(j0);
11047  IkReal x543 = (gconst7 * x542);
11048  IkReal x544 = ((1.0) * x541);
11049  IkReal x545 = (gconst8 * x542);
11050  IkReal x546 = (gconst8 * x544);
11051  evalcond[0] =
11052  ((((-1.0) * x546)) + x543);
11053  evalcond[1] =
11054  (gconst7 +
11055  ((new_r11 * x541)) +
11056  ((new_r01 * x542)));
11057  evalcond[2] =
11058  (x545 + ((gconst7 * x541)) +
11059  new_r11);
11060  evalcond[3] =
11061  (gconst8 +
11062  ((new_r11 * x542)) +
11063  (((-1.0) * new_r01 * x544)));
11064  evalcond[4] =
11065  ((((-1.0) * x545)) +
11066  (((-1.0) * gconst7 * x544)));
11067  evalcond[5] = ((((-1.0) * x546)) +
11068  x543 + new_r01);
11069  if (IKabs(evalcond[0]) >
11070  IKFAST_EVALCOND_THRESH ||
11071  IKabs(evalcond[1]) >
11072  IKFAST_EVALCOND_THRESH ||
11073  IKabs(evalcond[2]) >
11074  IKFAST_EVALCOND_THRESH ||
11075  IKabs(evalcond[3]) >
11076  IKFAST_EVALCOND_THRESH ||
11077  IKabs(evalcond[4]) >
11078  IKFAST_EVALCOND_THRESH ||
11079  IKabs(evalcond[5]) >
11080  IKFAST_EVALCOND_THRESH)
11081  {
11082  continue;
11083  }
11084  }
11085 
11086  {
11087  std::vector<
11088  IkSingleDOFSolutionBase<
11089  IkReal> >
11090  vinfos(7);
11091  vinfos[0].jointtype = 1;
11092  vinfos[0].foffset = j0;
11093  vinfos[0].indices[0] = _ij0[0];
11094  vinfos[0].indices[1] = _ij0[1];
11095  vinfos[0].maxsolutions = _nj0;
11096  vinfos[1].jointtype = 1;
11097  vinfos[1].foffset = j1;
11098  vinfos[1].indices[0] = _ij1[0];
11099  vinfos[1].indices[1] = _ij1[1];
11100  vinfos[1].maxsolutions = _nj1;
11101  vinfos[2].jointtype = 1;
11102  vinfos[2].foffset = j2;
11103  vinfos[2].indices[0] = _ij2[0];
11104  vinfos[2].indices[1] = _ij2[1];
11105  vinfos[2].maxsolutions = _nj2;
11106  vinfos[3].jointtype = 1;
11107  vinfos[3].foffset = j3;
11108  vinfos[3].indices[0] = _ij3[0];
11109  vinfos[3].indices[1] = _ij3[1];
11110  vinfos[3].maxsolutions = _nj3;
11111  vinfos[4].jointtype = 1;
11112  vinfos[4].foffset = j4;
11113  vinfos[4].indices[0] = _ij4[0];
11114  vinfos[4].indices[1] = _ij4[1];
11115  vinfos[4].maxsolutions = _nj4;
11116  vinfos[5].jointtype = 1;
11117  vinfos[5].foffset = j5;
11118  vinfos[5].indices[0] = _ij5[0];
11119  vinfos[5].indices[1] = _ij5[1];
11120  vinfos[5].maxsolutions = _nj5;
11121  vinfos[6].jointtype = 1;
11122  vinfos[6].foffset = j6;
11123  vinfos[6].indices[0] = _ij6[0];
11124  vinfos[6].indices[1] = _ij6[1];
11125  vinfos[6].maxsolutions = _nj6;
11126  std::vector<int> vfree(0);
11127  solutions.AddSolution(vinfos,
11128  vfree);
11129  }
11130  }
11131  }
11132  }
11133  }
11134  }
11135  } while (0);
11136  if (bgotonextstatement)
11137  {
11138  bool bgotonextstatement = true;
11139  do
11140  {
11141  evalcond[0] =
11142  ((IKabs(new_r10)) + (IKabs(new_r01)));
11143  if (IKabs(evalcond[0]) < 0.0000050000000000)
11144  {
11145  bgotonextstatement = false;
11146  {
11147  IkReal j0array[2], cj0array[2],
11148  sj0array[2];
11149  bool j0valid[2] = { false };
11150  _nj0 = 2;
11151  CheckValue<IkReal> x547 =
11152  IKPowWithIntegerCheck(gconst8, -1);
11153  if (!x547.valid)
11154  {
11155  continue;
11156  }
11157  cj0array[0] = (new_r00 * (x547.value));
11158  if (cj0array[0] >=
11159  -1 - IKFAST_SINCOS_THRESH &&
11160  cj0array[0] <=
11161  1 + IKFAST_SINCOS_THRESH)
11162  {
11163  j0valid[0] = j0valid[1] = true;
11164  j0array[0] = IKacos(cj0array[0]);
11165  sj0array[0] = IKsin(j0array[0]);
11166  cj0array[1] = cj0array[0];
11167  j0array[1] = -j0array[0];
11168  sj0array[1] = -sj0array[0];
11169  }
11170  else if (isnan(cj0array[0]))
11171  {
11172  // probably any value will work
11173  j0valid[0] = true;
11174  cj0array[0] = 1;
11175  sj0array[0] = 0;
11176  j0array[0] = 0;
11177  }
11178  for (int ij0 = 0; ij0 < 2; ++ij0)
11179  {
11180  if (!j0valid[ij0])
11181  {
11182  continue;
11183  }
11184  _ij0[0] = ij0;
11185  _ij0[1] = -1;
11186  for (int iij0 = ij0 + 1; iij0 < 2;
11187  ++iij0)
11188  {
11189  if (j0valid[iij0] &&
11190  IKabs(cj0array[ij0] -
11191  cj0array[iij0]) <
11192  IKFAST_SOLUTION_THRESH &&
11193  IKabs(sj0array[ij0] -
11194  sj0array[iij0]) <
11195  IKFAST_SOLUTION_THRESH)
11196  {
11197  j0valid[iij0] = false;
11198  _ij0[1] = iij0;
11199  break;
11200  }
11201  }
11202  j0 = j0array[ij0];
11203  cj0 = cj0array[ij0];
11204  sj0 = sj0array[ij0];
11205  {
11206  IkReal evalcond[6];
11207  IkReal x548 = IKsin(j0);
11208  IkReal x549 = IKcos(j0);
11209  IkReal x550 = ((-1.0) * x548);
11210  evalcond[0] = (new_r11 * x548);
11211  evalcond[1] = (new_r00 * x550);
11212  evalcond[2] = (gconst8 * x550);
11213  evalcond[3] =
11214  (gconst8 + ((new_r11 * x549)));
11215  evalcond[4] =
11216  (((gconst8 * x549)) + new_r11);
11217  evalcond[5] =
11218  (((new_r00 * x549)) +
11219  (((-1.0) * gconst8)));
11220  if (IKabs(evalcond[0]) >
11221  IKFAST_EVALCOND_THRESH ||
11222  IKabs(evalcond[1]) >
11223  IKFAST_EVALCOND_THRESH ||
11224  IKabs(evalcond[2]) >
11225  IKFAST_EVALCOND_THRESH ||
11226  IKabs(evalcond[3]) >
11227  IKFAST_EVALCOND_THRESH ||
11228  IKabs(evalcond[4]) >
11229  IKFAST_EVALCOND_THRESH ||
11230  IKabs(evalcond[5]) >
11231  IKFAST_EVALCOND_THRESH)
11232  {
11233  continue;
11234  }
11235  }
11236 
11237  {
11238  std::vector<IkSingleDOFSolutionBase<
11239  IkReal> >
11240  vinfos(7);
11241  vinfos[0].jointtype = 1;
11242  vinfos[0].foffset = j0;
11243  vinfos[0].indices[0] = _ij0[0];
11244  vinfos[0].indices[1] = _ij0[1];
11245  vinfos[0].maxsolutions = _nj0;
11246  vinfos[1].jointtype = 1;
11247  vinfos[1].foffset = j1;
11248  vinfos[1].indices[0] = _ij1[0];
11249  vinfos[1].indices[1] = _ij1[1];
11250  vinfos[1].maxsolutions = _nj1;
11251  vinfos[2].jointtype = 1;
11252  vinfos[2].foffset = j2;
11253  vinfos[2].indices[0] = _ij2[0];
11254  vinfos[2].indices[1] = _ij2[1];
11255  vinfos[2].maxsolutions = _nj2;
11256  vinfos[3].jointtype = 1;
11257  vinfos[3].foffset = j3;
11258  vinfos[3].indices[0] = _ij3[0];
11259  vinfos[3].indices[1] = _ij3[1];
11260  vinfos[3].maxsolutions = _nj3;
11261  vinfos[4].jointtype = 1;
11262  vinfos[4].foffset = j4;
11263  vinfos[4].indices[0] = _ij4[0];
11264  vinfos[4].indices[1] = _ij4[1];
11265  vinfos[4].maxsolutions = _nj4;
11266  vinfos[5].jointtype = 1;
11267  vinfos[5].foffset = j5;
11268  vinfos[5].indices[0] = _ij5[0];
11269  vinfos[5].indices[1] = _ij5[1];
11270  vinfos[5].maxsolutions = _nj5;
11271  vinfos[6].jointtype = 1;
11272  vinfos[6].foffset = j6;
11273  vinfos[6].indices[0] = _ij6[0];
11274  vinfos[6].indices[1] = _ij6[1];
11275  vinfos[6].maxsolutions = _nj6;
11276  std::vector<int> vfree(0);
11277  solutions.AddSolution(vinfos,
11278  vfree);
11279  }
11280  }
11281  }
11282  }
11283  } while (0);
11284  if (bgotonextstatement)
11285  {
11286  bool bgotonextstatement = true;
11287  do
11288  {
11289  evalcond[0] =
11290  ((IKabs(new_r00)) + (IKabs(new_r01)));
11291  if (IKabs(evalcond[0]) <
11292  0.0000050000000000)
11293  {
11294  bgotonextstatement = false;
11295  {
11296  IkReal j0eval[1];
11297  CheckValue<IkReal> x552 =
11298  IKatan2WithCheck(
11299  IkReal(0),
11300  IkReal(new_r11),
11301  IKFAST_ATAN2_MAGTHRESH);
11302  if (!x552.valid)
11303  {
11304  continue;
11305  }
11306  IkReal x551 = ((-1.0) * (x552.value));
11307  sj1 = 0;
11308  cj1 = -1.0;
11309  j1 = 3.14159265358979;
11310  sj2 = gconst7;
11311  cj2 = gconst8;
11312  j2 = x551;
11313  new_r00 = 0;
11314  new_r01 = 0;
11315  new_r12 = 0;
11316  new_r22 = 0;
11317  IkReal gconst6 = x551;
11318  IkReal gconst7 = 0;
11319  IkReal x553 =
11320  ((1.0) + (((-1.0) *
11321  (new_r10 * new_r10))));
11322  if (IKabs(x553) == 0)
11323  {
11324  continue;
11325  }
11326  IkReal gconst8 =
11327  (new_r11 * (pow(x553, -0.5)));
11328  j0eval[0] = ((IKabs(new_r11)) +
11329  (IKabs(new_r10)));
11330  if (IKabs(j0eval[0]) <
11331  0.0000010000000000)
11332  {
11333  {
11334  IkReal j0eval[1];
11335  CheckValue<IkReal> x555 =
11336  IKatan2WithCheck(
11337  IkReal(0),
11338  IkReal(new_r11),
11339  IKFAST_ATAN2_MAGTHRESH);
11340  if (!x555.valid)
11341  {
11342  continue;
11343  }
11344  IkReal x554 =
11345  ((-1.0) * (x555.value));
11346  sj1 = 0;
11347  cj1 = -1.0;
11348  j1 = 3.14159265358979;
11349  sj2 = gconst7;
11350  cj2 = gconst8;
11351  j2 = x554;
11352  new_r00 = 0;
11353  new_r01 = 0;
11354  new_r12 = 0;
11355  new_r22 = 0;
11356  IkReal gconst6 = x554;
11357  IkReal gconst7 = 0;
11358  IkReal x556 =
11359  ((1.0) +
11360  (((-1.0) *
11361  (new_r10 * new_r10))));
11362  if (IKabs(x556) == 0)
11363  {
11364  continue;
11365  }
11366  IkReal gconst8 =
11367  (new_r11 * (pow(x556, -0.5)));
11368  j0eval[0] = new_r11;
11369  if (IKabs(j0eval[0]) <
11370  0.0000010000000000)
11371  {
11372  {
11373  IkReal j0eval[2];
11374  CheckValue<IkReal> x558 =
11375  IKatan2WithCheck(
11376  IkReal(0),
11377  IkReal(new_r11),
11378  IKFAST_ATAN2_MAGTHRESH);
11379  if (!x558.valid)
11380  {
11381  continue;
11382  }
11383  IkReal x557 =
11384  ((-1.0) * (x558.value));
11385  sj1 = 0;
11386  cj1 = -1.0;
11387  j1 = 3.14159265358979;
11388  sj2 = gconst7;
11389  cj2 = gconst8;
11390  j2 = x557;
11391  new_r00 = 0;
11392  new_r01 = 0;
11393  new_r12 = 0;
11394  new_r22 = 0;
11395  IkReal gconst6 = x557;
11396  IkReal gconst7 = 0;
11397  IkReal x559 =
11398  ((1.0) +
11399  (((-1.0) *
11400  (new_r10 * new_r10))));
11401  if (IKabs(x559) == 0)
11402  {
11403  continue;
11404  }
11405  IkReal gconst8 =
11406  (new_r11 *
11407  (pow(x559, -0.5)));
11408  j0eval[0] = new_r10;
11409  j0eval[1] = new_r11;
11410  if (IKabs(j0eval[0]) <
11411  0.0000010000000000 ||
11412  IKabs(j0eval[1]) <
11413  0.0000010000000000)
11414  {
11415  continue; // 3 cases
11416  // reached
11417  }
11418  else
11419  {
11420  {
11421  IkReal j0array[1],
11422  cj0array[1],
11423  sj0array[1];
11424  bool j0valid[1] = {
11425  false
11426  };
11427  _nj0 = 1;
11428  CheckValue<IkReal> x560 =
11429  IKPowWithIntegerCheck(
11430  new_r10, -1);
11431  if (!x560.valid)
11432  {
11433  continue;
11434  }
11435  CheckValue<IkReal> x561 =
11436  IKPowWithIntegerCheck(
11437  new_r11, -1);
11438  if (!x561.valid)
11439  {
11440  continue;
11441  }
11442  if (IKabs(
11443  (gconst8 *
11444  (x560.value))) <
11445  IKFAST_ATAN2_MAGTHRESH &&
11446  IKabs((
11447  (-1.0) * gconst8 *
11448  (x561.value))) <
11449  IKFAST_ATAN2_MAGTHRESH &&
11450  IKabs(
11451  IKsqr((
11452  gconst8 *
11453  (x560.value))) +
11454  IKsqr((
11455  (-1.0) *
11456  gconst8 *
11457  (x561.value))) -
11458  1) <=
11459  IKFAST_SINCOS_THRESH)
11460  continue;
11461  j0array[0] = IKatan2(
11462  (gconst8 *
11463  (x560.value)),
11464  ((-1.0) * gconst8 *
11465  (x561.value)));
11466  sj0array[0] =
11467  IKsin(j0array[0]);
11468  cj0array[0] =
11469  IKcos(j0array[0]);
11470  if (j0array[0] > IKPI)
11471  {
11472  j0array[0] -= IK2PI;
11473  }
11474  else if (j0array[0] <
11475  -IKPI)
11476  {
11477  j0array[0] += IK2PI;
11478  }
11479  j0valid[0] = true;
11480  for (int ij0 = 0; ij0 < 1;
11481  ++ij0)
11482  {
11483  if (!j0valid[ij0])
11484  {
11485  continue;
11486  }
11487  _ij0[0] = ij0;
11488  _ij0[1] = -1;
11489  for (int iij0 = ij0 + 1;
11490  iij0 < 1;
11491  ++iij0)
11492  {
11493  if (j0valid[iij0] &&
11494  IKabs(
11495  cj0array
11496  [ij0] -
11497  cj0array
11498  [iij0]) <
11499  IKFAST_SOLUTION_THRESH &&
11500  IKabs(
11501  sj0array
11502  [ij0] -
11503  sj0array
11504  [iij0]) <
11505  IKFAST_SOLUTION_THRESH)
11506  {
11507  j0valid[iij0] =
11508  false;
11509  _ij0[1] = iij0;
11510  break;
11511  }
11512  }
11513  j0 = j0array[ij0];
11514  cj0 = cj0array[ij0];
11515  sj0 = sj0array[ij0];
11516  {
11517  IkReal evalcond[8];
11518  IkReal x562 =
11519  IKcos(j0);
11520  IkReal x563 =
11521  IKsin(j0);
11522  IkReal x564 =
11523  (gconst8 * x563);
11524  IkReal x565 =
11525  (gconst8 * x562);
11526  evalcond[0] =
11527  (new_r10 * x562);
11528  evalcond[1] =
11529  (new_r11 * x563);
11530  evalcond[2] =
11531  ((-1.0) * x565);
11532  evalcond[3] =
11533  ((-1.0) * x564);
11534  evalcond[4] =
11535  (gconst8 +
11536  ((new_r11 *
11537  x562)));
11538  evalcond[5] =
11539  (x565 + new_r11);
11540  evalcond[6] =
11541  ((((-1.0) *
11542  x564)) +
11543  new_r10);
11544  evalcond[7] =
11545  ((((-1.0) *
11546  gconst8)) +
11547  ((new_r10 *
11548  x563)));
11549  if (IKabs(
11550  evalcond[0]) >
11551  IKFAST_EVALCOND_THRESH ||
11552  IKabs(
11553  evalcond[1]) >
11554  IKFAST_EVALCOND_THRESH ||
11555  IKabs(
11556  evalcond[2]) >
11557  IKFAST_EVALCOND_THRESH ||
11558  IKabs(
11559  evalcond[3]) >
11560  IKFAST_EVALCOND_THRESH ||
11561  IKabs(
11562  evalcond[4]) >
11563  IKFAST_EVALCOND_THRESH ||
11564  IKabs(
11565  evalcond[5]) >
11566  IKFAST_EVALCOND_THRESH ||
11567  IKabs(
11568  evalcond[6]) >
11569  IKFAST_EVALCOND_THRESH ||
11570  IKabs(
11571  evalcond[7]) >
11572  IKFAST_EVALCOND_THRESH)
11573  {
11574  continue;
11575  }
11576  }
11577 
11578  {
11579  std::vector<
11580  IkSingleDOFSolutionBase<
11581  IkReal> >
11582  vinfos(7);
11583  vinfos[0].jointtype =
11584  1;
11585  vinfos[0].foffset =
11586  j0;
11587  vinfos[0].indices[0] =
11588  _ij0[0];
11589  vinfos[0].indices[1] =
11590  _ij0[1];
11591  vinfos[0]
11592  .maxsolutions =
11593  _nj0;
11594  vinfos[1].jointtype =
11595  1;
11596  vinfos[1].foffset =
11597  j1;
11598  vinfos[1].indices[0] =
11599  _ij1[0];
11600  vinfos[1].indices[1] =
11601  _ij1[1];
11602  vinfos[1]
11603  .maxsolutions =
11604  _nj1;
11605  vinfos[2].jointtype =
11606  1;
11607  vinfos[2].foffset =
11608  j2;
11609  vinfos[2].indices[0] =
11610  _ij2[0];
11611  vinfos[2].indices[1] =
11612  _ij2[1];
11613  vinfos[2]
11614  .maxsolutions =
11615  _nj2;
11616  vinfos[3].jointtype =
11617  1;
11618  vinfos[3].foffset =
11619  j3;
11620  vinfos[3].indices[0] =
11621  _ij3[0];
11622  vinfos[3].indices[1] =
11623  _ij3[1];
11624  vinfos[3]
11625  .maxsolutions =
11626  _nj3;
11627  vinfos[4].jointtype =
11628  1;
11629  vinfos[4].foffset =
11630  j4;
11631  vinfos[4].indices[0] =
11632  _ij4[0];
11633  vinfos[4].indices[1] =
11634  _ij4[1];
11635  vinfos[4]
11636  .maxsolutions =
11637  _nj4;
11638  vinfos[5].jointtype =
11639  1;
11640  vinfos[5].foffset =
11641  j5;
11642  vinfos[5].indices[0] =
11643  _ij5[0];
11644  vinfos[5].indices[1] =
11645  _ij5[1];
11646  vinfos[5]
11647  .maxsolutions =
11648  _nj5;
11649  vinfos[6].jointtype =
11650  1;
11651  vinfos[6].foffset =
11652  j6;
11653  vinfos[6].indices[0] =
11654  _ij6[0];
11655  vinfos[6].indices[1] =
11656  _ij6[1];
11657  vinfos[6]
11658  .maxsolutions =
11659  _nj6;
11660  std::vector<int>
11661  vfree(0);
11662  solutions.AddSolution(
11663  vinfos, vfree);
11664  }
11665  }
11666  }
11667  }
11668  }
11669  }
11670  else
11671  {
11672  {
11673  IkReal j0array[1],
11674  cj0array[1], sj0array[1];
11675  bool j0valid[1] = { false };
11676  _nj0 = 1;
11677  CheckValue<IkReal> x566 =
11678  IKPowWithIntegerCheck(
11679  gconst8, -1);
11680  if (!x566.valid)
11681  {
11682  continue;
11683  }
11684  CheckValue<IkReal> x567 =
11685  IKPowWithIntegerCheck(
11686  new_r11, -1);
11687  if (!x567.valid)
11688  {
11689  continue;
11690  }
11691  if (IKabs((new_r10 *
11692  (x566.value))) <
11693  IKFAST_ATAN2_MAGTHRESH &&
11694  IKabs(((-1.0) * gconst8 *
11695  (x567.value))) <
11696  IKFAST_ATAN2_MAGTHRESH &&
11697  IKabs(
11698  IKsqr(
11699  (new_r10 *
11700  (x566.value))) +
11701  IKsqr((
11702  (-1.0) * gconst8 *
11703  (x567.value))) -
11704  1) <=
11705  IKFAST_SINCOS_THRESH)
11706  continue;
11707  j0array[0] = IKatan2(
11708  (new_r10 * (x566.value)),
11709  ((-1.0) * gconst8 *
11710  (x567.value)));
11711  sj0array[0] =
11712  IKsin(j0array[0]);
11713  cj0array[0] =
11714  IKcos(j0array[0]);
11715  if (j0array[0] > IKPI)
11716  {
11717  j0array[0] -= IK2PI;
11718  }
11719  else if (j0array[0] < -IKPI)
11720  {
11721  j0array[0] += IK2PI;
11722  }
11723  j0valid[0] = true;
11724  for (int ij0 = 0; ij0 < 1;
11725  ++ij0)
11726  {
11727  if (!j0valid[ij0])
11728  {
11729  continue;
11730  }
11731  _ij0[0] = ij0;
11732  _ij0[1] = -1;
11733  for (int iij0 = ij0 + 1;
11734  iij0 < 1;
11735  ++iij0)
11736  {
11737  if (j0valid[iij0] &&
11738  IKabs(
11739  cj0array[ij0] -
11740  cj0array[iij0]) <
11741  IKFAST_SOLUTION_THRESH &&
11742  IKabs(
11743  sj0array[ij0] -
11744  sj0array[iij0]) <
11745  IKFAST_SOLUTION_THRESH)
11746  {
11747  j0valid[iij0] = false;
11748  _ij0[1] = iij0;
11749  break;
11750  }
11751  }
11752  j0 = j0array[ij0];
11753  cj0 = cj0array[ij0];
11754  sj0 = sj0array[ij0];
11755  {
11756  IkReal evalcond[8];
11757  IkReal x568 = IKcos(j0);
11758  IkReal x569 = IKsin(j0);
11759  IkReal x570 =
11760  (gconst8 * x569);
11761  IkReal x571 =
11762  (gconst8 * x568);
11763  evalcond[0] =
11764  (new_r10 * x568);
11765  evalcond[1] =
11766  (new_r11 * x569);
11767  evalcond[2] =
11768  ((-1.0) * x571);
11769  evalcond[3] =
11770  ((-1.0) * x570);
11771  evalcond[4] =
11772  (gconst8 +
11773  ((new_r11 * x568)));
11774  evalcond[5] =
11775  (x571 + new_r11);
11776  evalcond[6] =
11777  ((((-1.0) * x570)) +
11778  new_r10);
11779  evalcond[7] =
11780  ((((-1.0) *
11781  gconst8)) +
11782  ((new_r10 * x569)));
11783  if (IKabs(evalcond[0]) >
11784  IKFAST_EVALCOND_THRESH ||
11785  IKabs(evalcond[1]) >
11786  IKFAST_EVALCOND_THRESH ||
11787  IKabs(evalcond[2]) >
11788  IKFAST_EVALCOND_THRESH ||
11789  IKabs(evalcond[3]) >
11790  IKFAST_EVALCOND_THRESH ||
11791  IKabs(evalcond[4]) >
11792  IKFAST_EVALCOND_THRESH ||
11793  IKabs(evalcond[5]) >
11794  IKFAST_EVALCOND_THRESH ||
11795  IKabs(evalcond[6]) >
11796  IKFAST_EVALCOND_THRESH ||
11797  IKabs(evalcond[7]) >
11798  IKFAST_EVALCOND_THRESH)
11799  {
11800  continue;
11801  }
11802  }
11803 
11804  {
11805  std::vector<
11806  IkSingleDOFSolutionBase<
11807  IkReal> >
11808  vinfos(7);
11809  vinfos[0].jointtype = 1;
11810  vinfos[0].foffset = j0;
11811  vinfos[0].indices[0] =
11812  _ij0[0];
11813  vinfos[0].indices[1] =
11814  _ij0[1];
11815  vinfos[0].maxsolutions =
11816  _nj0;
11817  vinfos[1].jointtype = 1;
11818  vinfos[1].foffset = j1;
11819  vinfos[1].indices[0] =
11820  _ij1[0];
11821  vinfos[1].indices[1] =
11822  _ij1[1];
11823  vinfos[1].maxsolutions =
11824  _nj1;
11825  vinfos[2].jointtype = 1;
11826  vinfos[2].foffset = j2;
11827  vinfos[2].indices[0] =
11828  _ij2[0];
11829  vinfos[2].indices[1] =
11830  _ij2[1];
11831  vinfos[2].maxsolutions =
11832  _nj2;
11833  vinfos[3].jointtype = 1;
11834  vinfos[3].foffset = j3;
11835  vinfos[3].indices[0] =
11836  _ij3[0];
11837  vinfos[3].indices[1] =
11838  _ij3[1];
11839  vinfos[3].maxsolutions =
11840  _nj3;
11841  vinfos[4].jointtype = 1;
11842  vinfos[4].foffset = j4;
11843  vinfos[4].indices[0] =
11844  _ij4[0];
11845  vinfos[4].indices[1] =
11846  _ij4[1];
11847  vinfos[4].maxsolutions =
11848  _nj4;
11849  vinfos[5].jointtype = 1;
11850  vinfos[5].foffset = j5;
11851  vinfos[5].indices[0] =
11852  _ij5[0];
11853  vinfos[5].indices[1] =
11854  _ij5[1];
11855  vinfos[5].maxsolutions =
11856  _nj5;
11857  vinfos[6].jointtype = 1;
11858  vinfos[6].foffset = j6;
11859  vinfos[6].indices[0] =
11860  _ij6[0];
11861  vinfos[6].indices[1] =
11862  _ij6[1];
11863  vinfos[6].maxsolutions =
11864  _nj6;
11865  std::vector<int> vfree(0);
11866  solutions.AddSolution(
11867  vinfos, vfree);
11868  }
11869  }
11870  }
11871  }
11872  }
11873  }
11874  else
11875  {
11876  {
11877  IkReal j0array[1], cj0array[1],
11878  sj0array[1];
11879  bool j0valid[1] = { false };
11880  _nj0 = 1;
11881  CheckValue<IkReal> x572 =
11882  IKatan2WithCheck(
11883  IkReal(new_r10),
11884  IkReal(
11885  ((-1.0) * new_r11)),
11886  IKFAST_ATAN2_MAGTHRESH);
11887  if (!x572.valid)
11888  {
11889  continue;
11890  }
11891  CheckValue<IkReal> x573 =
11892  IKPowWithIntegerCheck(
11893  IKsign(gconst8), -1);
11894  if (!x573.valid)
11895  {
11896  continue;
11897  }
11898  j0array[0] =
11899  ((-1.5707963267949) +
11900  (x572.value) +
11901  (((1.5707963267949) *
11902  (x573.value))));
11903  sj0array[0] = IKsin(j0array[0]);
11904  cj0array[0] = IKcos(j0array[0]);
11905  if (j0array[0] > IKPI)
11906  {
11907  j0array[0] -= IK2PI;
11908  }
11909  else if (j0array[0] < -IKPI)
11910  {
11911  j0array[0] += IK2PI;
11912  }
11913  j0valid[0] = true;
11914  for (int ij0 = 0; ij0 < 1; ++ij0)
11915  {
11916  if (!j0valid[ij0])
11917  {
11918  continue;
11919  }
11920  _ij0[0] = ij0;
11921  _ij0[1] = -1;
11922  for (int iij0 = ij0 + 1;
11923  iij0 < 1;
11924  ++iij0)
11925  {
11926  if (j0valid[iij0] &&
11927  IKabs(cj0array[ij0] -
11928  cj0array[iij0]) <
11929  IKFAST_SOLUTION_THRESH &&
11930  IKabs(sj0array[ij0] -
11931  sj0array[iij0]) <
11932  IKFAST_SOLUTION_THRESH)
11933  {
11934  j0valid[iij0] = false;
11935  _ij0[1] = iij0;
11936  break;
11937  }
11938  }
11939  j0 = j0array[ij0];
11940  cj0 = cj0array[ij0];
11941  sj0 = sj0array[ij0];
11942  {
11943  IkReal evalcond[8];
11944  IkReal x574 = IKcos(j0);
11945  IkReal x575 = IKsin(j0);
11946  IkReal x576 =
11947  (gconst8 * x575);
11948  IkReal x577 =
11949  (gconst8 * x574);
11950  evalcond[0] =
11951  (new_r10 * x574);
11952  evalcond[1] =
11953  (new_r11 * x575);
11954  evalcond[2] = ((-1.0) * x577);
11955  evalcond[3] = ((-1.0) * x576);
11956  evalcond[4] =
11957  (((new_r11 * x574)) +
11958  gconst8);
11959  evalcond[5] =
11960  (x577 + new_r11);
11961  evalcond[6] =
11962  ((((-1.0) * x576)) +
11963  new_r10);
11964  evalcond[7] =
11965  (((new_r10 * x575)) +
11966  (((-1.0) * gconst8)));
11967  if (IKabs(evalcond[0]) >
11968  IKFAST_EVALCOND_THRESH ||
11969  IKabs(evalcond[1]) >
11970  IKFAST_EVALCOND_THRESH ||
11971  IKabs(evalcond[2]) >
11972  IKFAST_EVALCOND_THRESH ||
11973  IKabs(evalcond[3]) >
11974  IKFAST_EVALCOND_THRESH ||
11975  IKabs(evalcond[4]) >
11976  IKFAST_EVALCOND_THRESH ||
11977  IKabs(evalcond[5]) >
11978  IKFAST_EVALCOND_THRESH ||
11979  IKabs(evalcond[6]) >
11980  IKFAST_EVALCOND_THRESH ||
11981  IKabs(evalcond[7]) >
11982  IKFAST_EVALCOND_THRESH)
11983  {
11984  continue;
11985  }
11986  }
11987 
11988  {
11989  std::vector<
11990  IkSingleDOFSolutionBase<
11991  IkReal> >
11992  vinfos(7);
11993  vinfos[0].jointtype = 1;
11994  vinfos[0].foffset = j0;
11995  vinfos[0].indices[0] =
11996  _ij0[0];
11997  vinfos[0].indices[1] =
11998  _ij0[1];
11999  vinfos[0].maxsolutions = _nj0;
12000  vinfos[1].jointtype = 1;
12001  vinfos[1].foffset = j1;
12002  vinfos[1].indices[0] =
12003  _ij1[0];
12004  vinfos[1].indices[1] =
12005  _ij1[1];
12006  vinfos[1].maxsolutions = _nj1;
12007  vinfos[2].jointtype = 1;
12008  vinfos[2].foffset = j2;
12009  vinfos[2].indices[0] =
12010  _ij2[0];
12011  vinfos[2].indices[1] =
12012  _ij2[1];
12013  vinfos[2].maxsolutions = _nj2;
12014  vinfos[3].jointtype = 1;
12015  vinfos[3].foffset = j3;
12016  vinfos[3].indices[0] =
12017  _ij3[0];
12018  vinfos[3].indices[1] =
12019  _ij3[1];
12020  vinfos[3].maxsolutions = _nj3;
12021  vinfos[4].jointtype = 1;
12022  vinfos[4].foffset = j4;
12023  vinfos[4].indices[0] =
12024  _ij4[0];
12025  vinfos[4].indices[1] =
12026  _ij4[1];
12027  vinfos[4].maxsolutions = _nj4;
12028  vinfos[5].jointtype = 1;
12029  vinfos[5].foffset = j5;
12030  vinfos[5].indices[0] =
12031  _ij5[0];
12032  vinfos[5].indices[1] =
12033  _ij5[1];
12034  vinfos[5].maxsolutions = _nj5;
12035  vinfos[6].jointtype = 1;
12036  vinfos[6].foffset = j6;
12037  vinfos[6].indices[0] =
12038  _ij6[0];
12039  vinfos[6].indices[1] =
12040  _ij6[1];
12041  vinfos[6].maxsolutions = _nj6;
12042  std::vector<int> vfree(0);
12043  solutions.AddSolution(vinfos,
12044  vfree);
12045  }
12046  }
12047  }
12048  }
12049  }
12050  }
12051  } while (0);
12052  if (bgotonextstatement)
12053  {
12054  bool bgotonextstatement = true;
12055  do
12056  {
12057  evalcond[0] = IKabs(new_r01);
12058  if (IKabs(evalcond[0]) <
12059  0.0000050000000000)
12060  {
12061  bgotonextstatement = false;
12062  {
12063  IkReal j0eval[1];
12064  CheckValue<IkReal> x579 =
12065  IKatan2WithCheck(
12066  IkReal(0),
12067  IkReal(new_r11),
12068  IKFAST_ATAN2_MAGTHRESH);
12069  if (!x579.valid)
12070  {
12071  continue;
12072  }
12073  IkReal x578 =
12074  ((-1.0) * (x579.value));
12075  sj1 = 0;
12076  cj1 = -1.0;
12077  j1 = 3.14159265358979;
12078  sj2 = gconst7;
12079  cj2 = gconst8;
12080  j2 = x578;
12081  new_r01 = 0;
12082  IkReal gconst6 = x578;
12083  IkReal gconst7 = 0;
12084  IkReal x580 = new_r11 * new_r11;
12085  if (IKabs(x580) == 0)
12086  {
12087  continue;
12088  }
12089  IkReal gconst8 =
12090  (new_r11 * (pow(x580, -0.5)));
12091  j0eval[0] = ((IKabs(new_r10)) +
12092  (IKabs(new_r00)));
12093  if (IKabs(j0eval[0]) <
12094  0.0000010000000000)
12095  {
12096  {
12097  IkReal j0eval[1];
12098  CheckValue<IkReal> x582 =
12099  IKatan2WithCheck(
12100  IkReal(0),
12101  IkReal(new_r11),
12102  IKFAST_ATAN2_MAGTHRESH);
12103  if (!x582.valid)
12104  {
12105  continue;
12106  }
12107  IkReal x581 =
12108  ((-1.0) * (x582.value));
12109  sj1 = 0;
12110  cj1 = -1.0;
12111  j1 = 3.14159265358979;
12112  sj2 = gconst7;
12113  cj2 = gconst8;
12114  j2 = x581;
12115  new_r01 = 0;
12116  IkReal gconst6 = x581;
12117  IkReal gconst7 = 0;
12118  IkReal x583 = new_r11 * new_r11;
12119  if (IKabs(x583) == 0)
12120  {
12121  continue;
12122  }
12123  IkReal gconst8 =
12124  (new_r11 *
12125  (pow(x583, -0.5)));
12126  j0eval[0] = ((IKabs(new_r11)) +
12127  (IKabs(new_r10)));
12128  if (IKabs(j0eval[0]) <
12129  0.0000010000000000)
12130  {
12131  {
12132  IkReal j0eval[1];
12133  CheckValue<IkReal> x585 =
12134  IKatan2WithCheck(
12135  IkReal(0),
12136  IkReal(new_r11),
12137  IKFAST_ATAN2_MAGTHRESH);
12138  if (!x585.valid)
12139  {
12140  continue;
12141  }
12142  IkReal x584 =
12143  ((-1.0) * (x585.value));
12144  sj1 = 0;
12145  cj1 = -1.0;
12146  j1 = 3.14159265358979;
12147  sj2 = gconst7;
12148  cj2 = gconst8;
12149  j2 = x584;
12150  new_r01 = 0;
12151  IkReal gconst6 = x584;
12152  IkReal gconst7 = 0;
12153  IkReal x586 =
12154  new_r11 * new_r11;
12155  if (IKabs(x586) == 0)
12156  {
12157  continue;
12158  }
12159  IkReal gconst8 =
12160  (new_r11 *
12161  (pow(x586, -0.5)));
12162  j0eval[0] = new_r11;
12163  if (IKabs(j0eval[0]) <
12164  0.0000010000000000)
12165  {
12166  continue; // 3 cases
12167  // reached
12168  }
12169  else
12170  {
12171  {
12172  IkReal j0array[1],
12173  cj0array[1],
12174  sj0array[1];
12175  bool j0valid[1] = {
12176  false
12177  };
12178  _nj0 = 1;
12179  CheckValue<IkReal> x587 =
12180  IKPowWithIntegerCheck(
12181  gconst8, -1);
12182  if (!x587.valid)
12183  {
12184  continue;
12185  }
12186  CheckValue<IkReal> x588 =
12187  IKPowWithIntegerCheck(
12188  new_r11, -1);
12189  if (!x588.valid)
12190  {
12191  continue;
12192  }
12193  if (IKabs((
12194  new_r10 *
12195  (x587.value))) <
12196  IKFAST_ATAN2_MAGTHRESH &&
12197  IKabs((
12198  (-1.0) *
12199  gconst8 *
12200  (x588.value))) <
12201  IKFAST_ATAN2_MAGTHRESH &&
12202  IKabs(
12203  IKsqr((
12204  new_r10 *
12205  (x587.value))) +
12206  IKsqr((
12207  (-1.0) *
12208  gconst8 *
12209  (x588.value))) -
12210  1) <=
12211  IKFAST_SINCOS_THRESH)
12212  continue;
12213  j0array[0] = IKatan2(
12214  (new_r10 *
12215  (x587.value)),
12216  ((-1.0) * gconst8 *
12217  (x588.value)));
12218  sj0array[0] =
12219  IKsin(j0array[0]);
12220  cj0array[0] =
12221  IKcos(j0array[0]);
12222  if (j0array[0] > IKPI)
12223  {
12224  j0array[0] -= IK2PI;
12225  }
12226  else if (j0array[0] <
12227  -IKPI)
12228  {
12229  j0array[0] += IK2PI;
12230  }
12231  j0valid[0] = true;
12232  for (int ij0 = 0;
12233  ij0 < 1;
12234  ++ij0)
12235  {
12236  if (!j0valid[ij0])
12237  {
12238  continue;
12239  }
12240  _ij0[0] = ij0;
12241  _ij0[1] = -1;
12242  for (int iij0 =
12243  ij0 + 1;
12244  iij0 < 1;
12245  ++iij0)
12246  {
12247  if (j0valid[iij0] &&
12248  IKabs(
12249  cj0array
12250  [ij0] -
12251  cj0array
12252  [iij0]) <
12253  IKFAST_SOLUTION_THRESH &&
12254  IKabs(
12255  sj0array
12256  [ij0] -
12257  sj0array
12258  [iij0]) <
12259  IKFAST_SOLUTION_THRESH)
12260  {
12261  j0valid[iij0] =
12262  false;
12263  _ij0[1] = iij0;
12264  break;
12265  }
12266  }
12267  j0 = j0array[ij0];
12268  cj0 = cj0array[ij0];
12269  sj0 = sj0array[ij0];
12270  {
12271  IkReal evalcond[8];
12272  IkReal x589 =
12273  IKsin(j0);
12274  IkReal x590 =
12275  IKcos(j0);
12276  IkReal x591 =
12277  ((1.0) *
12278  gconst8);
12279  IkReal x592 =
12280  ((1.0) * x589);
12281  evalcond[0] =
12282  (new_r11 *
12283  x589);
12284  evalcond[1] =
12285  ((-1.0) *
12286  gconst8 *
12287  x589);
12288  evalcond[2] =
12289  (((new_r11 *
12290  x590)) +
12291  gconst8);
12292  evalcond[3] =
12293  (((gconst8 *
12294  x590)) +
12295  new_r11);
12296  evalcond[4] =
12297  ((((-1.0) *
12298  x589 *
12299  x591)) +
12300  new_r10);
12301  evalcond[5] =
12302  ((((-1.0) *
12303  x590 *
12304  x591)) +
12305  new_r00);
12306  evalcond[6] =
12307  ((((-1.0) *
12308  new_r00 *
12309  x592)) +
12310  ((new_r10 *
12311  x590)));
12312  evalcond[7] =
12313  (((new_r00 *
12314  x590)) +
12315  (((-1.0) *
12316  x591)) +
12317  ((new_r10 *
12318  x589)));
12319  if (IKabs(evalcond
12320  [0]) >
12321  IKFAST_EVALCOND_THRESH ||
12322  IKabs(evalcond
12323  [1]) >
12324  IKFAST_EVALCOND_THRESH ||
12325  IKabs(evalcond
12326  [2]) >
12327  IKFAST_EVALCOND_THRESH ||
12328  IKabs(evalcond
12329  [3]) >
12330  IKFAST_EVALCOND_THRESH ||
12331  IKabs(evalcond
12332  [4]) >
12333  IKFAST_EVALCOND_THRESH ||
12334  IKabs(evalcond
12335  [5]) >
12336  IKFAST_EVALCOND_THRESH ||
12337  IKabs(evalcond
12338  [6]) >
12339  IKFAST_EVALCOND_THRESH ||
12340  IKabs(evalcond
12341  [7]) >
12342  IKFAST_EVALCOND_THRESH)
12343  {
12344  continue;
12345  }
12346  }
12347 
12348  {
12349  std::vector<
12350  IkSingleDOFSolutionBase<
12351  IkReal> >
12352  vinfos(7);
12353  vinfos[0]
12354  .jointtype = 1;
12355  vinfos[0].foffset =
12356  j0;
12357  vinfos[0]
12358  .indices[0] =
12359  _ij0[0];
12360  vinfos[0]
12361  .indices[1] =
12362  _ij0[1];
12363  vinfos[0]
12364  .maxsolutions =
12365  _nj0;
12366  vinfos[1]
12367  .jointtype = 1;
12368  vinfos[1].foffset =
12369  j1;
12370  vinfos[1]
12371  .indices[0] =
12372  _ij1[0];
12373  vinfos[1]
12374  .indices[1] =
12375  _ij1[1];
12376  vinfos[1]
12377  .maxsolutions =
12378  _nj1;
12379  vinfos[2]
12380  .jointtype = 1;
12381  vinfos[2].foffset =
12382  j2;
12383  vinfos[2]
12384  .indices[0] =
12385  _ij2[0];
12386  vinfos[2]
12387  .indices[1] =
12388  _ij2[1];
12389  vinfos[2]
12390  .maxsolutions =
12391  _nj2;
12392  vinfos[3]
12393  .jointtype = 1;
12394  vinfos[3].foffset =
12395  j3;
12396  vinfos[3]
12397  .indices[0] =
12398  _ij3[0];
12399  vinfos[3]
12400  .indices[1] =
12401  _ij3[1];
12402  vinfos[3]
12403  .maxsolutions =
12404  _nj3;
12405  vinfos[4]
12406  .jointtype = 1;
12407  vinfos[4].foffset =
12408  j4;
12409  vinfos[4]
12410  .indices[0] =
12411  _ij4[0];
12412  vinfos[4]
12413  .indices[1] =
12414  _ij4[1];
12415  vinfos[4]
12416  .maxsolutions =
12417  _nj4;
12418  vinfos[5]
12419  .jointtype = 1;
12420  vinfos[5].foffset =
12421  j5;
12422  vinfos[5]
12423  .indices[0] =
12424  _ij5[0];
12425  vinfos[5]
12426  .indices[1] =
12427  _ij5[1];
12428  vinfos[5]
12429  .maxsolutions =
12430  _nj5;
12431  vinfos[6]
12432  .jointtype = 1;
12433  vinfos[6].foffset =
12434  j6;
12435  vinfos[6]
12436  .indices[0] =
12437  _ij6[0];
12438  vinfos[6]
12439  .indices[1] =
12440  _ij6[1];
12441  vinfos[6]
12442  .maxsolutions =
12443  _nj6;
12444  std::vector<int>
12445  vfree(0);
12446  solutions
12447  .AddSolution(
12448  vinfos,
12449  vfree);
12450  }
12451  }
12452  }
12453  }
12454  }
12455  }
12456  else
12457  {
12458  {
12459  IkReal j0array[1],
12460  cj0array[1],
12461  sj0array[1];
12462  bool j0valid[1] = { false };
12463  _nj0 = 1;
12464  CheckValue<IkReal> x593 =
12465  IKatan2WithCheck(
12466  IkReal(new_r10),
12467  IkReal(((-1.0) *
12468  new_r11)),
12469  IKFAST_ATAN2_MAGTHRESH);
12470  if (!x593.valid)
12471  {
12472  continue;
12473  }
12474  CheckValue<IkReal> x594 =
12475  IKPowWithIntegerCheck(
12476  IKsign(gconst8),
12477  -1);
12478  if (!x594.valid)
12479  {
12480  continue;
12481  }
12482  j0array[0] =
12483  ((-1.5707963267949) +
12484  (x593.value) +
12485  (((1.5707963267949) *
12486  (x594.value))));
12487  sj0array[0] =
12488  IKsin(j0array[0]);
12489  cj0array[0] =
12490  IKcos(j0array[0]);
12491  if (j0array[0] > IKPI)
12492  {
12493  j0array[0] -= IK2PI;
12494  }
12495  else if (j0array[0] < -IKPI)
12496  {
12497  j0array[0] += IK2PI;
12498  }
12499  j0valid[0] = true;
12500  for (int ij0 = 0; ij0 < 1;
12501  ++ij0)
12502  {
12503  if (!j0valid[ij0])
12504  {
12505  continue;
12506  }
12507  _ij0[0] = ij0;
12508  _ij0[1] = -1;
12509  for (int iij0 = ij0 + 1;
12510  iij0 < 1;
12511  ++iij0)
12512  {
12513  if (j0valid[iij0] &&
12514  IKabs(
12515  cj0array[ij0] -
12516  cj0array
12517  [iij0]) <
12518  IKFAST_SOLUTION_THRESH &&
12519  IKabs(
12520  sj0array[ij0] -
12521  sj0array
12522  [iij0]) <
12523  IKFAST_SOLUTION_THRESH)
12524  {
12525  j0valid[iij0] = false;
12526  _ij0[1] = iij0;
12527  break;
12528  }
12529  }
12530  j0 = j0array[ij0];
12531  cj0 = cj0array[ij0];
12532  sj0 = sj0array[ij0];
12533  {
12534  IkReal evalcond[8];
12535  IkReal x595 = IKsin(j0);
12536  IkReal x596 = IKcos(j0);
12537  IkReal x597 =
12538  ((1.0) * gconst8);
12539  IkReal x598 =
12540  ((1.0) * x595);
12541  evalcond[0] =
12542  (new_r11 * x595);
12543  evalcond[1] =
12544  ((-1.0) * gconst8 *
12545  x595);
12546  evalcond[2] =
12547  (((new_r11 *
12548  x596)) +
12549  gconst8);
12550  evalcond[3] =
12551  (((gconst8 *
12552  x596)) +
12553  new_r11);
12554  evalcond[4] =
12555  ((((-1.0) * x595 *
12556  x597)) +
12557  new_r10);
12558  evalcond[5] =
12559  ((((-1.0) * x596 *
12560  x597)) +
12561  new_r00);
12562  evalcond[6] =
12563  ((((-1.0) *
12564  new_r00 *
12565  x598)) +
12566  ((new_r10 *
12567  x596)));
12568  evalcond[7] =
12569  (((new_r10 *
12570  x595)) +
12571  ((new_r00 *
12572  x596)) +
12573  (((-1.0) * x597)));
12574  if (IKabs(evalcond[0]) >
12575  IKFAST_EVALCOND_THRESH ||
12576  IKabs(evalcond[1]) >
12577  IKFAST_EVALCOND_THRESH ||
12578  IKabs(evalcond[2]) >
12579  IKFAST_EVALCOND_THRESH ||
12580  IKabs(evalcond[3]) >
12581  IKFAST_EVALCOND_THRESH ||
12582  IKabs(evalcond[4]) >
12583  IKFAST_EVALCOND_THRESH ||
12584  IKabs(evalcond[5]) >
12585  IKFAST_EVALCOND_THRESH ||
12586  IKabs(evalcond[6]) >
12587  IKFAST_EVALCOND_THRESH ||
12588  IKabs(evalcond[7]) >
12589  IKFAST_EVALCOND_THRESH)
12590  {
12591  continue;
12592  }
12593  }
12594 
12595  {
12596  std::vector<
12597  IkSingleDOFSolutionBase<
12598  IkReal> >
12599  vinfos(7);
12600  vinfos[0].jointtype = 1;
12601  vinfos[0].foffset = j0;
12602  vinfos[0].indices[0] =
12603  _ij0[0];
12604  vinfos[0].indices[1] =
12605  _ij0[1];
12606  vinfos[0].maxsolutions =
12607  _nj0;
12608  vinfos[1].jointtype = 1;
12609  vinfos[1].foffset = j1;
12610  vinfos[1].indices[0] =
12611  _ij1[0];
12612  vinfos[1].indices[1] =
12613  _ij1[1];
12614  vinfos[1].maxsolutions =
12615  _nj1;
12616  vinfos[2].jointtype = 1;
12617  vinfos[2].foffset = j2;
12618  vinfos[2].indices[0] =
12619  _ij2[0];
12620  vinfos[2].indices[1] =
12621  _ij2[1];
12622  vinfos[2].maxsolutions =
12623  _nj2;
12624  vinfos[3].jointtype = 1;
12625  vinfos[3].foffset = j3;
12626  vinfos[3].indices[0] =
12627  _ij3[0];
12628  vinfos[3].indices[1] =
12629  _ij3[1];
12630  vinfos[3].maxsolutions =
12631  _nj3;
12632  vinfos[4].jointtype = 1;
12633  vinfos[4].foffset = j4;
12634  vinfos[4].indices[0] =
12635  _ij4[0];
12636  vinfos[4].indices[1] =
12637  _ij4[1];
12638  vinfos[4].maxsolutions =
12639  _nj4;
12640  vinfos[5].jointtype = 1;
12641  vinfos[5].foffset = j5;
12642  vinfos[5].indices[0] =
12643  _ij5[0];
12644  vinfos[5].indices[1] =
12645  _ij5[1];
12646  vinfos[5].maxsolutions =
12647  _nj5;
12648  vinfos[6].jointtype = 1;
12649  vinfos[6].foffset = j6;
12650  vinfos[6].indices[0] =
12651  _ij6[0];
12652  vinfos[6].indices[1] =
12653  _ij6[1];
12654  vinfos[6].maxsolutions =
12655  _nj6;
12656  std::vector<int> vfree(
12657  0);
12658  solutions.AddSolution(
12659  vinfos, vfree);
12660  }
12661  }
12662  }
12663  }
12664  }
12665  }
12666  else
12667  {
12668  {
12669  IkReal j0array[1], cj0array[1],
12670  sj0array[1];
12671  bool j0valid[1] = { false };
12672  _nj0 = 1;
12673  CheckValue<IkReal> x599 =
12674  IKPowWithIntegerCheck(
12675  IKsign(gconst8), -1);
12676  if (!x599.valid)
12677  {
12678  continue;
12679  }
12680  CheckValue<IkReal> x600 =
12681  IKatan2WithCheck(
12682  IkReal(new_r10),
12683  IkReal(new_r00),
12684  IKFAST_ATAN2_MAGTHRESH);
12685  if (!x600.valid)
12686  {
12687  continue;
12688  }
12689  j0array[0] =
12690  ((-1.5707963267949) +
12691  (((1.5707963267949) *
12692  (x599.value))) +
12693  (x600.value));
12694  sj0array[0] = IKsin(j0array[0]);
12695  cj0array[0] = IKcos(j0array[0]);
12696  if (j0array[0] > IKPI)
12697  {
12698  j0array[0] -= IK2PI;
12699  }
12700  else if (j0array[0] < -IKPI)
12701  {
12702  j0array[0] += IK2PI;
12703  }
12704  j0valid[0] = true;
12705  for (int ij0 = 0; ij0 < 1;
12706  ++ij0)
12707  {
12708  if (!j0valid[ij0])
12709  {
12710  continue;
12711  }
12712  _ij0[0] = ij0;
12713  _ij0[1] = -1;
12714  for (int iij0 = ij0 + 1;
12715  iij0 < 1;
12716  ++iij0)
12717  {
12718  if (j0valid[iij0] &&
12719  IKabs(cj0array[ij0] -
12720  cj0array[iij0]) <
12721  IKFAST_SOLUTION_THRESH &&
12722  IKabs(sj0array[ij0] -
12723  sj0array[iij0]) <
12724  IKFAST_SOLUTION_THRESH)
12725  {
12726  j0valid[iij0] = false;
12727  _ij0[1] = iij0;
12728  break;
12729  }
12730  }
12731  j0 = j0array[ij0];
12732  cj0 = cj0array[ij0];
12733  sj0 = sj0array[ij0];
12734  {
12735  IkReal evalcond[8];
12736  IkReal x601 = IKsin(j0);
12737  IkReal x602 = IKcos(j0);
12738  IkReal x603 =
12739  ((1.0) * gconst8);
12740  IkReal x604 =
12741  ((1.0) * x601);
12742  evalcond[0] =
12743  (new_r11 * x601);
12744  evalcond[1] =
12745  ((-1.0) * gconst8 *
12746  x601);
12747  evalcond[2] =
12748  (gconst8 +
12749  ((new_r11 * x602)));
12750  evalcond[3] =
12751  (((gconst8 * x602)) +
12752  new_r11);
12753  evalcond[4] =
12754  (new_r10 +
12755  (((-1.0) * x601 *
12756  x603)));
12757  evalcond[5] =
12758  ((((-1.0) * x602 *
12759  x603)) +
12760  new_r00);
12761  evalcond[6] =
12762  (((new_r10 * x602)) +
12763  (((-1.0) * new_r00 *
12764  x604)));
12765  evalcond[7] =
12766  (((new_r10 * x601)) +
12767  ((new_r00 * x602)) +
12768  (((-1.0) * x603)));
12769  if (IKabs(evalcond[0]) >
12770  IKFAST_EVALCOND_THRESH ||
12771  IKabs(evalcond[1]) >
12772  IKFAST_EVALCOND_THRESH ||
12773  IKabs(evalcond[2]) >
12774  IKFAST_EVALCOND_THRESH ||
12775  IKabs(evalcond[3]) >
12776  IKFAST_EVALCOND_THRESH ||
12777  IKabs(evalcond[4]) >
12778  IKFAST_EVALCOND_THRESH ||
12779  IKabs(evalcond[5]) >
12780  IKFAST_EVALCOND_THRESH ||
12781  IKabs(evalcond[6]) >
12782  IKFAST_EVALCOND_THRESH ||
12783  IKabs(evalcond[7]) >
12784  IKFAST_EVALCOND_THRESH)
12785  {
12786  continue;
12787  }
12788  }
12789 
12790  {
12791  std::vector<
12792  IkSingleDOFSolutionBase<
12793  IkReal> >
12794  vinfos(7);
12795  vinfos[0].jointtype = 1;
12796  vinfos[0].foffset = j0;
12797  vinfos[0].indices[0] =
12798  _ij0[0];
12799  vinfos[0].indices[1] =
12800  _ij0[1];
12801  vinfos[0].maxsolutions =
12802  _nj0;
12803  vinfos[1].jointtype = 1;
12804  vinfos[1].foffset = j1;
12805  vinfos[1].indices[0] =
12806  _ij1[0];
12807  vinfos[1].indices[1] =
12808  _ij1[1];
12809  vinfos[1].maxsolutions =
12810  _nj1;
12811  vinfos[2].jointtype = 1;
12812  vinfos[2].foffset = j2;
12813  vinfos[2].indices[0] =
12814  _ij2[0];
12815  vinfos[2].indices[1] =
12816  _ij2[1];
12817  vinfos[2].maxsolutions =
12818  _nj2;
12819  vinfos[3].jointtype = 1;
12820  vinfos[3].foffset = j3;
12821  vinfos[3].indices[0] =
12822  _ij3[0];
12823  vinfos[3].indices[1] =
12824  _ij3[1];
12825  vinfos[3].maxsolutions =
12826  _nj3;
12827  vinfos[4].jointtype = 1;
12828  vinfos[4].foffset = j4;
12829  vinfos[4].indices[0] =
12830  _ij4[0];
12831  vinfos[4].indices[1] =
12832  _ij4[1];
12833  vinfos[4].maxsolutions =
12834  _nj4;
12835  vinfos[5].jointtype = 1;
12836  vinfos[5].foffset = j5;
12837  vinfos[5].indices[0] =
12838  _ij5[0];
12839  vinfos[5].indices[1] =
12840  _ij5[1];
12841  vinfos[5].maxsolutions =
12842  _nj5;
12843  vinfos[6].jointtype = 1;
12844  vinfos[6].foffset = j6;
12845  vinfos[6].indices[0] =
12846  _ij6[0];
12847  vinfos[6].indices[1] =
12848  _ij6[1];
12849  vinfos[6].maxsolutions =
12850  _nj6;
12851  std::vector<int> vfree(0);
12852  solutions.AddSolution(
12853  vinfos, vfree);
12854  }
12855  }
12856  }
12857  }
12858  }
12859  }
12860  } while (0);
12861  if (bgotonextstatement)
12862  {
12863  bool bgotonextstatement = true;
12864  do
12865  {
12866  if (1)
12867  {
12868  bgotonextstatement = false;
12869  continue; // branch miss [j0]
12870  }
12871  } while (0);
12872  if (bgotonextstatement)
12873  {
12874  }
12875  }
12876  }
12877  }
12878  }
12879  }
12880  }
12881  }
12882  else
12883  {
12884  {
12885  IkReal j0array[1], cj0array[1], sj0array[1];
12886  bool j0valid[1] = { false };
12887  _nj0 = 1;
12888  IkReal x605 = ((1.0) * new_r11);
12889  CheckValue<IkReal> x606 = IKatan2WithCheck(
12890  IkReal(((((-1.0) * new_r01 * x605)) +
12891  ((gconst7 * gconst8)))),
12892  IkReal(((((-1.0) * (gconst7 * gconst7))) +
12893  (new_r11 * new_r11))),
12894  IKFAST_ATAN2_MAGTHRESH);
12895  if (!x606.valid)
12896  {
12897  continue;
12898  }
12899  CheckValue<IkReal> x607 = IKPowWithIntegerCheck(
12900  IKsign(((((-1.0) * gconst8 * x605)) +
12901  ((gconst7 * new_r01)))),
12902  -1);
12903  if (!x607.valid)
12904  {
12905  continue;
12906  }
12907  j0array[0] =
12908  ((-1.5707963267949) + (x606.value) +
12909  (((1.5707963267949) * (x607.value))));
12910  sj0array[0] = IKsin(j0array[0]);
12911  cj0array[0] = IKcos(j0array[0]);
12912  if (j0array[0] > IKPI)
12913  {
12914  j0array[0] -= IK2PI;
12915  }
12916  else if (j0array[0] < -IKPI)
12917  {
12918  j0array[0] += IK2PI;
12919  }
12920  j0valid[0] = true;
12921  for (int ij0 = 0; ij0 < 1; ++ij0)
12922  {
12923  if (!j0valid[ij0])
12924  {
12925  continue;
12926  }
12927  _ij0[0] = ij0;
12928  _ij0[1] = -1;
12929  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
12930  {
12931  if (j0valid[iij0] &&
12932  IKabs(cj0array[ij0] - cj0array[iij0]) <
12933  IKFAST_SOLUTION_THRESH &&
12934  IKabs(sj0array[ij0] - sj0array[iij0]) <
12935  IKFAST_SOLUTION_THRESH)
12936  {
12937  j0valid[iij0] = false;
12938  _ij0[1] = iij0;
12939  break;
12940  }
12941  }
12942  j0 = j0array[ij0];
12943  cj0 = cj0array[ij0];
12944  sj0 = sj0array[ij0];
12945  {
12946  IkReal evalcond[8];
12947  IkReal x608 = IKcos(j0);
12948  IkReal x609 = IKsin(j0);
12949  IkReal x610 = (gconst7 * x608);
12950  IkReal x611 = ((1.0) * x609);
12951  IkReal x612 = (gconst8 * x608);
12952  IkReal x613 = (gconst8 * x611);
12953  evalcond[0] = (gconst7 + ((new_r11 * x609)) +
12954  ((new_r01 * x608)));
12955  evalcond[1] =
12956  (((gconst7 * x609)) + x612 + new_r11);
12957  evalcond[2] = (gconst7 + ((new_r10 * x608)) +
12958  (((-1.0) * new_r00 * x611)));
12959  evalcond[3] = (gconst8 + ((new_r11 * x608)) +
12960  (((-1.0) * new_r01 * x611)));
12961  evalcond[4] =
12962  ((((-1.0) * x613)) + x610 + new_r10);
12963  evalcond[5] =
12964  ((((-1.0) * x613)) + x610 + new_r01);
12965  evalcond[6] =
12966  ((((-1.0) * gconst8)) +
12967  ((new_r10 * x609)) + ((new_r00 * x608)));
12968  evalcond[7] = ((((-1.0) * x612)) + new_r00 +
12969  (((-1.0) * gconst7 * x611)));
12970  if (IKabs(evalcond[0]) >
12971  IKFAST_EVALCOND_THRESH ||
12972  IKabs(evalcond[1]) >
12973  IKFAST_EVALCOND_THRESH ||
12974  IKabs(evalcond[2]) >
12975  IKFAST_EVALCOND_THRESH ||
12976  IKabs(evalcond[3]) >
12977  IKFAST_EVALCOND_THRESH ||
12978  IKabs(evalcond[4]) >
12979  IKFAST_EVALCOND_THRESH ||
12980  IKabs(evalcond[5]) >
12981  IKFAST_EVALCOND_THRESH ||
12982  IKabs(evalcond[6]) >
12983  IKFAST_EVALCOND_THRESH ||
12984  IKabs(evalcond[7]) >
12985  IKFAST_EVALCOND_THRESH)
12986  {
12987  continue;
12988  }
12989  }
12990 
12991  {
12992  std::vector<IkSingleDOFSolutionBase<IkReal> >
12993  vinfos(7);
12994  vinfos[0].jointtype = 1;
12995  vinfos[0].foffset = j0;
12996  vinfos[0].indices[0] = _ij0[0];
12997  vinfos[0].indices[1] = _ij0[1];
12998  vinfos[0].maxsolutions = _nj0;
12999  vinfos[1].jointtype = 1;
13000  vinfos[1].foffset = j1;
13001  vinfos[1].indices[0] = _ij1[0];
13002  vinfos[1].indices[1] = _ij1[1];
13003  vinfos[1].maxsolutions = _nj1;
13004  vinfos[2].jointtype = 1;
13005  vinfos[2].foffset = j2;
13006  vinfos[2].indices[0] = _ij2[0];
13007  vinfos[2].indices[1] = _ij2[1];
13008  vinfos[2].maxsolutions = _nj2;
13009  vinfos[3].jointtype = 1;
13010  vinfos[3].foffset = j3;
13011  vinfos[3].indices[0] = _ij3[0];
13012  vinfos[3].indices[1] = _ij3[1];
13013  vinfos[3].maxsolutions = _nj3;
13014  vinfos[4].jointtype = 1;
13015  vinfos[4].foffset = j4;
13016  vinfos[4].indices[0] = _ij4[0];
13017  vinfos[4].indices[1] = _ij4[1];
13018  vinfos[4].maxsolutions = _nj4;
13019  vinfos[5].jointtype = 1;
13020  vinfos[5].foffset = j5;
13021  vinfos[5].indices[0] = _ij5[0];
13022  vinfos[5].indices[1] = _ij5[1];
13023  vinfos[5].maxsolutions = _nj5;
13024  vinfos[6].jointtype = 1;
13025  vinfos[6].foffset = j6;
13026  vinfos[6].indices[0] = _ij6[0];
13027  vinfos[6].indices[1] = _ij6[1];
13028  vinfos[6].maxsolutions = _nj6;
13029  std::vector<int> vfree(0);
13030  solutions.AddSolution(vinfos, vfree);
13031  }
13032  }
13033  }
13034  }
13035  }
13036  }
13037  else
13038  {
13039  {
13040  IkReal j0array[1], cj0array[1], sj0array[1];
13041  bool j0valid[1] = { false };
13042  _nj0 = 1;
13043  IkReal x614 = ((1.0) * new_r11);
13044  CheckValue<IkReal> x615 = IKPowWithIntegerCheck(
13045  IKsign(
13046  ((new_r01 * new_r01) + (new_r11 * new_r11))),
13047  -1);
13048  if (!x615.valid)
13049  {
13050  continue;
13051  }
13052  CheckValue<IkReal> x616 = IKatan2WithCheck(
13053  IkReal((((gconst8 * new_r01)) +
13054  (((-1.0) * gconst7 * x614)))),
13055  IkReal(((((-1.0) * gconst7 * new_r01)) +
13056  (((-1.0) * gconst8 * x614)))),
13057  IKFAST_ATAN2_MAGTHRESH);
13058  if (!x616.valid)
13059  {
13060  continue;
13061  }
13062  j0array[0] = ((-1.5707963267949) +
13063  (((1.5707963267949) * (x615.value))) +
13064  (x616.value));
13065  sj0array[0] = IKsin(j0array[0]);
13066  cj0array[0] = IKcos(j0array[0]);
13067  if (j0array[0] > IKPI)
13068  {
13069  j0array[0] -= IK2PI;
13070  }
13071  else if (j0array[0] < -IKPI)
13072  {
13073  j0array[0] += IK2PI;
13074  }
13075  j0valid[0] = true;
13076  for (int ij0 = 0; ij0 < 1; ++ij0)
13077  {
13078  if (!j0valid[ij0])
13079  {
13080  continue;
13081  }
13082  _ij0[0] = ij0;
13083  _ij0[1] = -1;
13084  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13085  {
13086  if (j0valid[iij0] &&
13087  IKabs(cj0array[ij0] - cj0array[iij0]) <
13088  IKFAST_SOLUTION_THRESH &&
13089  IKabs(sj0array[ij0] - sj0array[iij0]) <
13090  IKFAST_SOLUTION_THRESH)
13091  {
13092  j0valid[iij0] = false;
13093  _ij0[1] = iij0;
13094  break;
13095  }
13096  }
13097  j0 = j0array[ij0];
13098  cj0 = cj0array[ij0];
13099  sj0 = sj0array[ij0];
13100  {
13101  IkReal evalcond[8];
13102  IkReal x617 = IKcos(j0);
13103  IkReal x618 = IKsin(j0);
13104  IkReal x619 = (gconst7 * x617);
13105  IkReal x620 = ((1.0) * x618);
13106  IkReal x621 = (gconst8 * x617);
13107  IkReal x622 = (gconst8 * x620);
13108  evalcond[0] = (gconst7 + ((new_r11 * x618)) +
13109  ((new_r01 * x617)));
13110  evalcond[1] =
13111  (((gconst7 * x618)) + x621 + new_r11);
13112  evalcond[2] = ((((-1.0) * new_r00 * x620)) +
13113  gconst7 + ((new_r10 * x617)));
13114  evalcond[3] = ((((-1.0) * new_r01 * x620)) +
13115  gconst8 + ((new_r11 * x617)));
13116  evalcond[4] =
13117  ((((-1.0) * x622)) + x619 + new_r10);
13118  evalcond[5] =
13119  ((((-1.0) * x622)) + x619 + new_r01);
13120  evalcond[6] =
13121  ((((-1.0) * gconst8)) + ((new_r10 * x618)) +
13122  ((new_r00 * x617)));
13123  evalcond[7] = ((((-1.0) * gconst7 * x620)) +
13124  (((-1.0) * x621)) + new_r00);
13125  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13126  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13127  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13128  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13129  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13130  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13131  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13132  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13133  {
13134  continue;
13135  }
13136  }
13137 
13138  {
13139  std::vector<IkSingleDOFSolutionBase<IkReal> >
13140  vinfos(7);
13141  vinfos[0].jointtype = 1;
13142  vinfos[0].foffset = j0;
13143  vinfos[0].indices[0] = _ij0[0];
13144  vinfos[0].indices[1] = _ij0[1];
13145  vinfos[0].maxsolutions = _nj0;
13146  vinfos[1].jointtype = 1;
13147  vinfos[1].foffset = j1;
13148  vinfos[1].indices[0] = _ij1[0];
13149  vinfos[1].indices[1] = _ij1[1];
13150  vinfos[1].maxsolutions = _nj1;
13151  vinfos[2].jointtype = 1;
13152  vinfos[2].foffset = j2;
13153  vinfos[2].indices[0] = _ij2[0];
13154  vinfos[2].indices[1] = _ij2[1];
13155  vinfos[2].maxsolutions = _nj2;
13156  vinfos[3].jointtype = 1;
13157  vinfos[3].foffset = j3;
13158  vinfos[3].indices[0] = _ij3[0];
13159  vinfos[3].indices[1] = _ij3[1];
13160  vinfos[3].maxsolutions = _nj3;
13161  vinfos[4].jointtype = 1;
13162  vinfos[4].foffset = j4;
13163  vinfos[4].indices[0] = _ij4[0];
13164  vinfos[4].indices[1] = _ij4[1];
13165  vinfos[4].maxsolutions = _nj4;
13166  vinfos[5].jointtype = 1;
13167  vinfos[5].foffset = j5;
13168  vinfos[5].indices[0] = _ij5[0];
13169  vinfos[5].indices[1] = _ij5[1];
13170  vinfos[5].maxsolutions = _nj5;
13171  vinfos[6].jointtype = 1;
13172  vinfos[6].foffset = j6;
13173  vinfos[6].indices[0] = _ij6[0];
13174  vinfos[6].indices[1] = _ij6[1];
13175  vinfos[6].maxsolutions = _nj6;
13176  std::vector<int> vfree(0);
13177  solutions.AddSolution(vinfos, vfree);
13178  }
13179  }
13180  }
13181  }
13182  }
13183  }
13184  else
13185  {
13186  {
13187  IkReal j0array[1], cj0array[1], sj0array[1];
13188  bool j0valid[1] = { false };
13189  _nj0 = 1;
13190  IkReal x623 = ((1.0) * gconst7);
13191  CheckValue<IkReal> x624 = IKatan2WithCheck(
13192  IkReal(((((-1.0) * new_r10 * x623)) +
13193  ((gconst7 * new_r01)))),
13194  IkReal(((((-1.0) * new_r00 * x623)) +
13195  (((-1.0) * new_r11 * x623)))),
13196  IKFAST_ATAN2_MAGTHRESH);
13197  if (!x624.valid)
13198  {
13199  continue;
13200  }
13201  CheckValue<IkReal> x625 =
13202  IKPowWithIntegerCheck(IKsign((((new_r10 * new_r11)) +
13203  ((new_r00 * new_r01)))),
13204  -1);
13205  if (!x625.valid)
13206  {
13207  continue;
13208  }
13209  j0array[0] = ((-1.5707963267949) + (x624.value) +
13210  (((1.5707963267949) * (x625.value))));
13211  sj0array[0] = IKsin(j0array[0]);
13212  cj0array[0] = IKcos(j0array[0]);
13213  if (j0array[0] > IKPI)
13214  {
13215  j0array[0] -= IK2PI;
13216  }
13217  else if (j0array[0] < -IKPI)
13218  {
13219  j0array[0] += IK2PI;
13220  }
13221  j0valid[0] = true;
13222  for (int ij0 = 0; ij0 < 1; ++ij0)
13223  {
13224  if (!j0valid[ij0])
13225  {
13226  continue;
13227  }
13228  _ij0[0] = ij0;
13229  _ij0[1] = -1;
13230  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13231  {
13232  if (j0valid[iij0] &&
13233  IKabs(cj0array[ij0] - cj0array[iij0]) <
13234  IKFAST_SOLUTION_THRESH &&
13235  IKabs(sj0array[ij0] - sj0array[iij0]) <
13236  IKFAST_SOLUTION_THRESH)
13237  {
13238  j0valid[iij0] = false;
13239  _ij0[1] = iij0;
13240  break;
13241  }
13242  }
13243  j0 = j0array[ij0];
13244  cj0 = cj0array[ij0];
13245  sj0 = sj0array[ij0];
13246  {
13247  IkReal evalcond[8];
13248  IkReal x626 = IKcos(j0);
13249  IkReal x627 = IKsin(j0);
13250  IkReal x628 = (gconst7 * x626);
13251  IkReal x629 = ((1.0) * x627);
13252  IkReal x630 = (gconst8 * x626);
13253  IkReal x631 = (gconst8 * x629);
13254  evalcond[0] = (gconst7 + ((new_r01 * x626)) +
13255  ((new_r11 * x627)));
13256  evalcond[1] = (x630 + new_r11 + ((gconst7 * x627)));
13257  evalcond[2] = ((((-1.0) * new_r00 * x629)) + gconst7 +
13258  ((new_r10 * x626)));
13259  evalcond[3] = ((((-1.0) * new_r01 * x629)) + gconst8 +
13260  ((new_r11 * x626)));
13261  evalcond[4] = ((((-1.0) * x631)) + x628 + new_r10);
13262  evalcond[5] = ((((-1.0) * x631)) + x628 + new_r01);
13263  evalcond[6] =
13264  ((((-1.0) * gconst8)) + ((new_r00 * x626)) +
13265  ((new_r10 * x627)));
13266  evalcond[7] = ((((-1.0) * gconst7 * x629)) +
13267  (((-1.0) * x630)) + new_r00);
13268  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13269  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13270  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13271  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13272  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13273  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13274  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13275  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13276  {
13277  continue;
13278  }
13279  }
13280 
13281  {
13282  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
13283  7);
13284  vinfos[0].jointtype = 1;
13285  vinfos[0].foffset = j0;
13286  vinfos[0].indices[0] = _ij0[0];
13287  vinfos[0].indices[1] = _ij0[1];
13288  vinfos[0].maxsolutions = _nj0;
13289  vinfos[1].jointtype = 1;
13290  vinfos[1].foffset = j1;
13291  vinfos[1].indices[0] = _ij1[0];
13292  vinfos[1].indices[1] = _ij1[1];
13293  vinfos[1].maxsolutions = _nj1;
13294  vinfos[2].jointtype = 1;
13295  vinfos[2].foffset = j2;
13296  vinfos[2].indices[0] = _ij2[0];
13297  vinfos[2].indices[1] = _ij2[1];
13298  vinfos[2].maxsolutions = _nj2;
13299  vinfos[3].jointtype = 1;
13300  vinfos[3].foffset = j3;
13301  vinfos[3].indices[0] = _ij3[0];
13302  vinfos[3].indices[1] = _ij3[1];
13303  vinfos[3].maxsolutions = _nj3;
13304  vinfos[4].jointtype = 1;
13305  vinfos[4].foffset = j4;
13306  vinfos[4].indices[0] = _ij4[0];
13307  vinfos[4].indices[1] = _ij4[1];
13308  vinfos[4].maxsolutions = _nj4;
13309  vinfos[5].jointtype = 1;
13310  vinfos[5].foffset = j5;
13311  vinfos[5].indices[0] = _ij5[0];
13312  vinfos[5].indices[1] = _ij5[1];
13313  vinfos[5].maxsolutions = _nj5;
13314  vinfos[6].jointtype = 1;
13315  vinfos[6].foffset = j6;
13316  vinfos[6].indices[0] = _ij6[0];
13317  vinfos[6].indices[1] = _ij6[1];
13318  vinfos[6].maxsolutions = _nj6;
13319  std::vector<int> vfree(0);
13320  solutions.AddSolution(vinfos, vfree);
13321  }
13322  }
13323  }
13324  }
13325  }
13326  }
13327  } while (0);
13328  if (bgotonextstatement)
13329  {
13330  bool bgotonextstatement = true;
13331  do
13332  {
13333  IkReal x633 = ((new_r01 * new_r01) + (new_r11 * new_r11));
13334  if (IKabs(x633) == 0)
13335  {
13336  continue;
13337  }
13338  IkReal x632 = pow(x633, -0.5);
13339  CheckValue<IkReal> x634 = IKatan2WithCheck(
13340  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13341  if (!x634.valid)
13342  {
13343  continue;
13344  }
13345  IkReal gconst9 =
13346  ((3.14159265358979) + (((-1.0) * (x634.value))));
13347  IkReal gconst10 = ((1.0) * new_r01 * x632);
13348  IkReal gconst11 = ((-1.0) * new_r11 * x632);
13349  CheckValue<IkReal> x635 = IKatan2WithCheck(
13350  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13351  if (!x635.valid)
13352  {
13353  continue;
13354  }
13355  evalcond[0] =
13356  ((-3.14159265358979) +
13357  (IKfmod(
13358  ((3.14159265358979) +
13359  (IKabs(((-3.14159265358979) + (x635.value) + j2)))),
13360  6.28318530717959)));
13361  if (IKabs(evalcond[0]) < 0.0000050000000000)
13362  {
13363  bgotonextstatement = false;
13364  {
13365  IkReal j0eval[3];
13366  CheckValue<IkReal> x638 =
13367  IKatan2WithCheck(IkReal(new_r01),
13368  IkReal(new_r11),
13369  IKFAST_ATAN2_MAGTHRESH);
13370  if (!x638.valid)
13371  {
13372  continue;
13373  }
13374  IkReal x636 = ((1.0) * (x638.value));
13375  IkReal x637 = x632;
13376  sj1 = 0;
13377  cj1 = -1.0;
13378  j1 = 3.14159265358979;
13379  sj2 = gconst10;
13380  cj2 = gconst11;
13381  j2 = ((3.14159265) + (((-1.0) * x636)));
13382  IkReal gconst9 = ((3.14159265358979) + (((-1.0) * x636)));
13383  IkReal gconst10 = ((1.0) * new_r01 * x637);
13384  IkReal gconst11 = ((-1.0) * new_r11 * x637);
13385  IkReal x639 = new_r01 * new_r01;
13386  IkReal x640 =
13387  (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
13388  IkReal x641 = x632;
13389  IkReal x642 = ((1.0) * new_r01 * x641);
13390  j0eval[0] = x640;
13391  j0eval[1] = ((IKabs((((x639 * x641)) +
13392  (((-1.0) * new_r10 * x642))))) +
13393  (IKabs(((((-1.0) * new_r11 * x642)) +
13394  (((-1.0) * new_r00 * x642))))));
13395  j0eval[2] = IKsign(x640);
13396  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13397  IKabs(j0eval[1]) < 0.0000010000000000 ||
13398  IKabs(j0eval[2]) < 0.0000010000000000)
13399  {
13400  {
13401  IkReal j0eval[2];
13402  CheckValue<IkReal> x645 =
13403  IKatan2WithCheck(IkReal(new_r01),
13404  IkReal(new_r11),
13405  IKFAST_ATAN2_MAGTHRESH);
13406  if (!x645.valid)
13407  {
13408  continue;
13409  }
13410  IkReal x643 = ((1.0) * (x645.value));
13411  IkReal x644 = x632;
13412  sj1 = 0;
13413  cj1 = -1.0;
13414  j1 = 3.14159265358979;
13415  sj2 = gconst10;
13416  cj2 = gconst11;
13417  j2 = ((3.14159265) + (((-1.0) * x643)));
13418  IkReal gconst9 =
13419  ((3.14159265358979) + (((-1.0) * x643)));
13420  IkReal gconst10 = ((1.0) * new_r01 * x644);
13421  IkReal gconst11 = ((-1.0) * new_r11 * x644);
13422  IkReal x646 =
13423  ((new_r01 * new_r01) + (new_r11 * new_r11));
13424  j0eval[0] = x646;
13425  j0eval[1] = IKsign(x646);
13426  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13427  IKabs(j0eval[1]) < 0.0000010000000000)
13428  {
13429  {
13430  IkReal j0eval[1];
13431  CheckValue<IkReal> x649 =
13432  IKatan2WithCheck(IkReal(new_r01),
13433  IkReal(new_r11),
13434  IKFAST_ATAN2_MAGTHRESH);
13435  if (!x649.valid)
13436  {
13437  continue;
13438  }
13439  IkReal x647 = ((1.0) * (x649.value));
13440  IkReal x648 = x632;
13441  sj1 = 0;
13442  cj1 = -1.0;
13443  j1 = 3.14159265358979;
13444  sj2 = gconst10;
13445  cj2 = gconst11;
13446  j2 = ((3.14159265) + (((-1.0) * x647)));
13447  IkReal gconst9 =
13448  ((3.14159265358979) + (((-1.0) * x647)));
13449  IkReal gconst10 = ((1.0) * new_r01 * x648);
13450  IkReal gconst11 = ((-1.0) * new_r11 * x648);
13451  IkReal x650 = new_r01 * new_r01;
13452  IkReal x651 = new_r11 * new_r11;
13453  IkReal x652 = ((1.0) * x650);
13454  CheckValue<IkReal> x658 =
13455  IKPowWithIntegerCheck((x650 + x651), -1);
13456  if (!x658.valid)
13457  {
13458  continue;
13459  }
13460  IkReal x653 = x658.value;
13461  CheckValue<IkReal> x659 = IKPowWithIntegerCheck(
13462  ((((-1.0) * x651)) + (((-1.0) * x652))), -1);
13463  if (!x659.valid)
13464  {
13465  continue;
13466  }
13467  IkReal x654 = x659.value;
13468  IkReal x655 = ((1.0) * x654);
13469  IkReal x656 = (new_r11 * x655);
13470  IkReal x657 = (new_r01 * x655);
13471  j0eval[0] =
13472  ((IKabs(((((-1.0) * new_r01 * x656)) +
13473  (((-1.0) * x656 *
13474  (new_r01 * new_r01 * new_r01))) +
13475  (((-1.0) * new_r01 * x656 *
13476  (new_r11 * new_r11)))))) +
13477  (IKabs((((x650 * x651 * x653)) +
13478  ((x653 * (x651 * x651))) +
13479  (((-1.0) * x652 * x653))))));
13480  if (IKabs(j0eval[0]) < 0.0000010000000000)
13481  {
13482  {
13483  IkReal evalcond[3];
13484  bool bgotonextstatement = true;
13485  do
13486  {
13487  evalcond[0] =
13488  ((IKabs(new_r11)) + (IKabs(new_r00)));
13489  if (IKabs(evalcond[0]) < 0.0000050000000000)
13490  {
13491  bgotonextstatement = false;
13492  {
13493  IkReal j0eval[1];
13494  CheckValue<IkReal> x661 =
13495  IKatan2WithCheck(
13496  IkReal(new_r01),
13497  IkReal(0),
13498  IKFAST_ATAN2_MAGTHRESH);
13499  if (!x661.valid)
13500  {
13501  continue;
13502  }
13503  IkReal x660 = ((1.0) * (x661.value));
13504  sj1 = 0;
13505  cj1 = -1.0;
13506  j1 = 3.14159265358979;
13507  sj2 = gconst10;
13508  cj2 = gconst11;
13509  j2 = ((3.14159265) + (((-1.0) * x660)));
13510  new_r11 = 0;
13511  new_r00 = 0;
13512  IkReal gconst9 = ((3.14159265358979) +
13513  (((-1.0) * x660)));
13514  IkReal x662 = new_r01 * new_r01;
13515  if (IKabs(x662) == 0)
13516  {
13517  continue;
13518  }
13519  IkReal gconst10 =
13520  ((1.0) * new_r01 * (pow(x662, -0.5)));
13521  IkReal gconst11 = 0;
13522  j0eval[0] = new_r10;
13523  if (IKabs(j0eval[0]) < 0.0000010000000000)
13524  {
13525  {
13526  IkReal j0array[2], cj0array[2],
13527  sj0array[2];
13528  bool j0valid[2] = { false };
13529  _nj0 = 2;
13530  CheckValue<IkReal> x663 =
13531  IKPowWithIntegerCheck(gconst10,
13532  -1);
13533  if (!x663.valid)
13534  {
13535  continue;
13536  }
13537  cj0array[0] =
13538  ((-1.0) * new_r10 * (x663.value));
13539  if (cj0array[0] >=
13540  -1 - IKFAST_SINCOS_THRESH &&
13541  cj0array[0] <=
13542  1 + IKFAST_SINCOS_THRESH)
13543  {
13544  j0valid[0] = j0valid[1] = true;
13545  j0array[0] = IKacos(cj0array[0]);
13546  sj0array[0] = IKsin(j0array[0]);
13547  cj0array[1] = cj0array[0];
13548  j0array[1] = -j0array[0];
13549  sj0array[1] = -sj0array[0];
13550  }
13551  else if (isnan(cj0array[0]))
13552  {
13553  // probably any value will work
13554  j0valid[0] = true;
13555  cj0array[0] = 1;
13556  sj0array[0] = 0;
13557  j0array[0] = 0;
13558  }
13559  for (int ij0 = 0; ij0 < 2; ++ij0)
13560  {
13561  if (!j0valid[ij0])
13562  {
13563  continue;
13564  }
13565  _ij0[0] = ij0;
13566  _ij0[1] = -1;
13567  for (int iij0 = ij0 + 1; iij0 < 2;
13568  ++iij0)
13569  {
13570  if (j0valid[iij0] &&
13571  IKabs(cj0array[ij0] -
13572  cj0array[iij0]) <
13573  IKFAST_SOLUTION_THRESH &&
13574  IKabs(sj0array[ij0] -
13575  sj0array[iij0]) <
13576  IKFAST_SOLUTION_THRESH)
13577  {
13578  j0valid[iij0] = false;
13579  _ij0[1] = iij0;
13580  break;
13581  }
13582  }
13583  j0 = j0array[ij0];
13584  cj0 = cj0array[ij0];
13585  sj0 = sj0array[ij0];
13586  {
13587  IkReal evalcond[6];
13588  IkReal x664 = IKsin(j0);
13589  IkReal x665 = IKcos(j0);
13590  IkReal x666 = ((-1.0) * x664);
13591  evalcond[0] = (new_r10 * x664);
13592  evalcond[1] = (new_r01 * x666);
13593  evalcond[2] = (gconst10 * x666);
13594  evalcond[3] =
13595  (gconst10 +
13596  ((new_r10 * x665)));
13597  evalcond[4] =
13598  (gconst10 +
13599  ((new_r01 * x665)));
13600  evalcond[5] =
13601  (new_r01 +
13602  ((gconst10 * x665)));
13603  if (IKabs(evalcond[0]) >
13604  IKFAST_EVALCOND_THRESH ||
13605  IKabs(evalcond[1]) >
13606  IKFAST_EVALCOND_THRESH ||
13607  IKabs(evalcond[2]) >
13608  IKFAST_EVALCOND_THRESH ||
13609  IKabs(evalcond[3]) >
13610  IKFAST_EVALCOND_THRESH ||
13611  IKabs(evalcond[4]) >
13612  IKFAST_EVALCOND_THRESH ||
13613  IKabs(evalcond[5]) >
13614  IKFAST_EVALCOND_THRESH)
13615  {
13616  continue;
13617  }
13618  }
13619 
13620  {
13621  std::vector<
13622  IkSingleDOFSolutionBase<
13623  IkReal> >
13624  vinfos(7);
13625  vinfos[0].jointtype = 1;
13626  vinfos[0].foffset = j0;
13627  vinfos[0].indices[0] = _ij0[0];
13628  vinfos[0].indices[1] = _ij0[1];
13629  vinfos[0].maxsolutions = _nj0;
13630  vinfos[1].jointtype = 1;
13631  vinfos[1].foffset = j1;
13632  vinfos[1].indices[0] = _ij1[0];
13633  vinfos[1].indices[1] = _ij1[1];
13634  vinfos[1].maxsolutions = _nj1;
13635  vinfos[2].jointtype = 1;
13636  vinfos[2].foffset = j2;
13637  vinfos[2].indices[0] = _ij2[0];
13638  vinfos[2].indices[1] = _ij2[1];
13639  vinfos[2].maxsolutions = _nj2;
13640  vinfos[3].jointtype = 1;
13641  vinfos[3].foffset = j3;
13642  vinfos[3].indices[0] = _ij3[0];
13643  vinfos[3].indices[1] = _ij3[1];
13644  vinfos[3].maxsolutions = _nj3;
13645  vinfos[4].jointtype = 1;
13646  vinfos[4].foffset = j4;
13647  vinfos[4].indices[0] = _ij4[0];
13648  vinfos[4].indices[1] = _ij4[1];
13649  vinfos[4].maxsolutions = _nj4;
13650  vinfos[5].jointtype = 1;
13651  vinfos[5].foffset = j5;
13652  vinfos[5].indices[0] = _ij5[0];
13653  vinfos[5].indices[1] = _ij5[1];
13654  vinfos[5].maxsolutions = _nj5;
13655  vinfos[6].jointtype = 1;
13656  vinfos[6].foffset = j6;
13657  vinfos[6].indices[0] = _ij6[0];
13658  vinfos[6].indices[1] = _ij6[1];
13659  vinfos[6].maxsolutions = _nj6;
13660  std::vector<int> vfree(0);
13661  solutions.AddSolution(vinfos,
13662  vfree);
13663  }
13664  }
13665  }
13666  }
13667  else
13668  {
13669  {
13670  IkReal j0array[2], cj0array[2],
13671  sj0array[2];
13672  bool j0valid[2] = { false };
13673  _nj0 = 2;
13674  CheckValue<IkReal> x667 =
13675  IKPowWithIntegerCheck(new_r10,
13676  -1);
13677  if (!x667.valid)
13678  {
13679  continue;
13680  }
13681  cj0array[0] = ((-1.0) * gconst10 *
13682  (x667.value));
13683  if (cj0array[0] >=
13684  -1 - IKFAST_SINCOS_THRESH &&
13685  cj0array[0] <=
13686  1 + IKFAST_SINCOS_THRESH)
13687  {
13688  j0valid[0] = j0valid[1] = true;
13689  j0array[0] = IKacos(cj0array[0]);
13690  sj0array[0] = IKsin(j0array[0]);
13691  cj0array[1] = cj0array[0];
13692  j0array[1] = -j0array[0];
13693  sj0array[1] = -sj0array[0];
13694  }
13695  else if (isnan(cj0array[0]))
13696  {
13697  // probably any value will work
13698  j0valid[0] = true;
13699  cj0array[0] = 1;
13700  sj0array[0] = 0;
13701  j0array[0] = 0;
13702  }
13703  for (int ij0 = 0; ij0 < 2; ++ij0)
13704  {
13705  if (!j0valid[ij0])
13706  {
13707  continue;
13708  }
13709  _ij0[0] = ij0;
13710  _ij0[1] = -1;
13711  for (int iij0 = ij0 + 1; iij0 < 2;
13712  ++iij0)
13713  {
13714  if (j0valid[iij0] &&
13715  IKabs(cj0array[ij0] -
13716  cj0array[iij0]) <
13717  IKFAST_SOLUTION_THRESH &&
13718  IKabs(sj0array[ij0] -
13719  sj0array[iij0]) <
13720  IKFAST_SOLUTION_THRESH)
13721  {
13722  j0valid[iij0] = false;
13723  _ij0[1] = iij0;
13724  break;
13725  }
13726  }
13727  j0 = j0array[ij0];
13728  cj0 = cj0array[ij0];
13729  sj0 = sj0array[ij0];
13730  {
13731  IkReal evalcond[6];
13732  IkReal x668 = IKsin(j0);
13733  IkReal x669 = IKcos(j0);
13734  IkReal x670 = (gconst10 * x669);
13735  IkReal x671 = ((-1.0) * x668);
13736  evalcond[0] = (new_r10 * x668);
13737  evalcond[1] = (new_r01 * x671);
13738  evalcond[2] = (gconst10 * x671);
13739  evalcond[3] = (x670 + new_r10);
13740  evalcond[4] =
13741  (gconst10 +
13742  ((new_r01 * x669)));
13743  evalcond[5] = (x670 + new_r01);
13744  if (IKabs(evalcond[0]) >
13745  IKFAST_EVALCOND_THRESH ||
13746  IKabs(evalcond[1]) >
13747  IKFAST_EVALCOND_THRESH ||
13748  IKabs(evalcond[2]) >
13749  IKFAST_EVALCOND_THRESH ||
13750  IKabs(evalcond[3]) >
13751  IKFAST_EVALCOND_THRESH ||
13752  IKabs(evalcond[4]) >
13753  IKFAST_EVALCOND_THRESH ||
13754  IKabs(evalcond[5]) >
13755  IKFAST_EVALCOND_THRESH)
13756  {
13757  continue;
13758  }
13759  }
13760 
13761  {
13762  std::vector<
13763  IkSingleDOFSolutionBase<
13764  IkReal> >
13765  vinfos(7);
13766  vinfos[0].jointtype = 1;
13767  vinfos[0].foffset = j0;
13768  vinfos[0].indices[0] = _ij0[0];
13769  vinfos[0].indices[1] = _ij0[1];
13770  vinfos[0].maxsolutions = _nj0;
13771  vinfos[1].jointtype = 1;
13772  vinfos[1].foffset = j1;
13773  vinfos[1].indices[0] = _ij1[0];
13774  vinfos[1].indices[1] = _ij1[1];
13775  vinfos[1].maxsolutions = _nj1;
13776  vinfos[2].jointtype = 1;
13777  vinfos[2].foffset = j2;
13778  vinfos[2].indices[0] = _ij2[0];
13779  vinfos[2].indices[1] = _ij2[1];
13780  vinfos[2].maxsolutions = _nj2;
13781  vinfos[3].jointtype = 1;
13782  vinfos[3].foffset = j3;
13783  vinfos[3].indices[0] = _ij3[0];
13784  vinfos[3].indices[1] = _ij3[1];
13785  vinfos[3].maxsolutions = _nj3;
13786  vinfos[4].jointtype = 1;
13787  vinfos[4].foffset = j4;
13788  vinfos[4].indices[0] = _ij4[0];
13789  vinfos[4].indices[1] = _ij4[1];
13790  vinfos[4].maxsolutions = _nj4;
13791  vinfos[5].jointtype = 1;
13792  vinfos[5].foffset = j5;
13793  vinfos[5].indices[0] = _ij5[0];
13794  vinfos[5].indices[1] = _ij5[1];
13795  vinfos[5].maxsolutions = _nj5;
13796  vinfos[6].jointtype = 1;
13797  vinfos[6].foffset = j6;
13798  vinfos[6].indices[0] = _ij6[0];
13799  vinfos[6].indices[1] = _ij6[1];
13800  vinfos[6].maxsolutions = _nj6;
13801  std::vector<int> vfree(0);
13802  solutions.AddSolution(vinfos,
13803  vfree);
13804  }
13805  }
13806  }
13807  }
13808  }
13809  }
13810  } while (0);
13811  if (bgotonextstatement)
13812  {
13813  bool bgotonextstatement = true;
13814  do
13815  {
13816  evalcond[0] =
13817  ((IKabs(new_r10)) + (IKabs(new_r00)));
13818  evalcond[1] = gconst10;
13819  evalcond[2] = gconst11;
13820  if (IKabs(evalcond[0]) <
13821  0.0000050000000000 &&
13822  IKabs(evalcond[1]) <
13823  0.0000050000000000 &&
13824  IKabs(evalcond[2]) < 0.0000050000000000)
13825  {
13826  bgotonextstatement = false;
13827  {
13828  IkReal j0eval[3];
13829  CheckValue<IkReal> x673 =
13830  IKatan2WithCheck(
13831  IkReal(new_r01),
13832  IkReal(new_r11),
13833  IKFAST_ATAN2_MAGTHRESH);
13834  if (!x673.valid)
13835  {
13836  continue;
13837  }
13838  IkReal x672 = ((1.0) * (x673.value));
13839  sj1 = 0;
13840  cj1 = -1.0;
13841  j1 = 3.14159265358979;
13842  sj2 = gconst10;
13843  cj2 = gconst11;
13844  j2 = ((3.14159265) + (((-1.0) * x672)));
13845  new_r00 = 0;
13846  new_r10 = 0;
13847  new_r21 = 0;
13848  new_r22 = 0;
13849  IkReal gconst9 = ((3.14159265358979) +
13850  (((-1.0) * x672)));
13851  IkReal gconst10 = ((1.0) * new_r01);
13852  IkReal gconst11 = ((-1.0) * new_r11);
13853  j0eval[0] = 1.0;
13854  j0eval[1] = 1.0;
13855  j0eval[2] =
13856  ((IKabs(((1.0) +
13857  (((-1.0) * (new_r01 *
13858  new_r01)))))) +
13859  (IKabs(
13860  ((1.0) * new_r01 * new_r11))));
13861  if (IKabs(j0eval[0]) <
13862  0.0000010000000000 ||
13863  IKabs(j0eval[1]) <
13864  0.0000010000000000 ||
13865  IKabs(j0eval[2]) <
13866  0.0000010000000000)
13867  {
13868  {
13869  IkReal j0eval[3];
13870  CheckValue<IkReal> x675 =
13871  IKatan2WithCheck(
13872  IkReal(new_r01),
13873  IkReal(new_r11),
13874  IKFAST_ATAN2_MAGTHRESH);
13875  if (!x675.valid)
13876  {
13877  continue;
13878  }
13879  IkReal x674 =
13880  ((1.0) * (x675.value));
13881  sj1 = 0;
13882  cj1 = -1.0;
13883  j1 = 3.14159265358979;
13884  sj2 = gconst10;
13885  cj2 = gconst11;
13886  j2 = ((3.14159265) +
13887  (((-1.0) * x674)));
13888  new_r00 = 0;
13889  new_r10 = 0;
13890  new_r21 = 0;
13891  new_r22 = 0;
13892  IkReal gconst9 =
13893  ((3.14159265358979) +
13894  (((-1.0) * x674)));
13895  IkReal gconst10 = ((1.0) * new_r01);
13896  IkReal gconst11 =
13897  ((-1.0) * new_r11);
13898  j0eval[0] = -1.0;
13899  j0eval[1] = -1.0;
13900  j0eval[2] =
13901  ((IKabs(((-1.0) + (new_r01 *
13902  new_r01)))) +
13903  (IKabs(((1.0) * new_r01 *
13904  new_r11))));
13905  if (IKabs(j0eval[0]) <
13906  0.0000010000000000 ||
13907  IKabs(j0eval[1]) <
13908  0.0000010000000000 ||
13909  IKabs(j0eval[2]) <
13910  0.0000010000000000)
13911  {
13912  {
13913  IkReal j0eval[3];
13914  CheckValue<IkReal> x677 =
13915  IKatan2WithCheck(
13916  IkReal(new_r01),
13917  IkReal(new_r11),
13918  IKFAST_ATAN2_MAGTHRESH);
13919  if (!x677.valid)
13920  {
13921  continue;
13922  }
13923  IkReal x676 =
13924  ((1.0) * (x677.value));
13925  sj1 = 0;
13926  cj1 = -1.0;
13927  j1 = 3.14159265358979;
13928  sj2 = gconst10;
13929  cj2 = gconst11;
13930  j2 = ((3.14159265) +
13931  (((-1.0) * x676)));
13932  new_r00 = 0;
13933  new_r10 = 0;
13934  new_r21 = 0;
13935  new_r22 = 0;
13936  IkReal gconst9 =
13937  ((3.14159265358979) +
13938  (((-1.0) * x676)));
13939  IkReal gconst10 =
13940  ((1.0) * new_r01);
13941  IkReal gconst11 =
13942  ((-1.0) * new_r11);
13943  j0eval[0] = 1.0;
13944  j0eval[1] = 1.0;
13945  j0eval[2] =
13946  ((IKabs(((2.0) * new_r01 *
13947  new_r11))) +
13948  (IKabs(((1.0) +
13949  (((-2.0) *
13950  (new_r01 *
13951  new_r01)))))));
13952  if (IKabs(j0eval[0]) <
13953  0.0000010000000000 ||
13954  IKabs(j0eval[1]) <
13955  0.0000010000000000 ||
13956  IKabs(j0eval[2]) <
13957  0.0000010000000000)
13958  {
13959  continue; // 3 cases reached
13960  }
13961  else
13962  {
13963  {
13964  IkReal j0array[1],
13965  cj0array[1],
13966  sj0array[1];
13967  bool j0valid[1] = { false };
13968  _nj0 = 1;
13969  IkReal x678 =
13970  ((1.0) * new_r11);
13971  CheckValue<IkReal> x679 =
13972  IKatan2WithCheck(
13973  IkReal(
13974  (((gconst11 *
13975  new_r01)) +
13976  (((-1.0) *
13977  gconst10 *
13978  x678)))),
13979  IkReal(
13980  ((((-1.0) *
13981  gconst11 *
13982  x678)) +
13983  (((-1.0) *
13984  gconst10 *
13985  new_r01)))),
13986  IKFAST_ATAN2_MAGTHRESH);
13987  if (!x679.valid)
13988  {
13989  continue;
13990  }
13991  CheckValue<IkReal> x680 =
13992  IKPowWithIntegerCheck(
13993  IKsign(((new_r01 *
13994  new_r01) +
13995  (new_r11 *
13996  new_r11))),
13997  -1);
13998  if (!x680.valid)
13999  {
14000  continue;
14001  }
14002  j0array[0] =
14003  ((-1.5707963267949) +
14004  (x679.value) +
14005  (((1.5707963267949) *
14006  (x680.value))));
14007  sj0array[0] =
14008  IKsin(j0array[0]);
14009  cj0array[0] =
14010  IKcos(j0array[0]);
14011  if (j0array[0] > IKPI)
14012  {
14013  j0array[0] -= IK2PI;
14014  }
14015  else if (j0array[0] < -IKPI)
14016  {
14017  j0array[0] += IK2PI;
14018  }
14019  j0valid[0] = true;
14020  for (int ij0 = 0; ij0 < 1;
14021  ++ij0)
14022  {
14023  if (!j0valid[ij0])
14024  {
14025  continue;
14026  }
14027  _ij0[0] = ij0;
14028  _ij0[1] = -1;
14029  for (int iij0 = ij0 + 1;
14030  iij0 < 1;
14031  ++iij0)
14032  {
14033  if (j0valid[iij0] &&
14034  IKabs(
14035  cj0array[ij0] -
14036  cj0array
14037  [iij0]) <
14038  IKFAST_SOLUTION_THRESH &&
14039  IKabs(
14040  sj0array[ij0] -
14041  sj0array
14042  [iij0]) <
14043  IKFAST_SOLUTION_THRESH)
14044  {
14045  j0valid[iij0] = false;
14046  _ij0[1] = iij0;
14047  break;
14048  }
14049  }
14050  j0 = j0array[ij0];
14051  cj0 = cj0array[ij0];
14052  sj0 = sj0array[ij0];
14053  {
14054  IkReal evalcond[6];
14055  IkReal x681 = IKsin(j0);
14056  IkReal x682 = IKcos(j0);
14057  IkReal x683 =
14058  (gconst10 * x682);
14059  IkReal x684 =
14060  (gconst11 * x682);
14061  IkReal x685 =
14062  (gconst10 * x681);
14063  IkReal x686 =
14064  ((1.0) * x681);
14065  IkReal x687 =
14066  (gconst11 * x686);
14067  evalcond[0] =
14068  ((((-1.0) * x687)) +
14069  x683);
14070  evalcond[1] =
14071  (gconst10 +
14072  ((new_r01 *
14073  x682)) +
14074  ((new_r11 *
14075  x681)));
14076  evalcond[2] =
14077  (x685 + x684 +
14078  new_r11);
14079  evalcond[3] =
14080  ((((-1.0) *
14081  new_r01 *
14082  x686)) +
14083  gconst11 +
14084  ((new_r11 *
14085  x682)));
14086  evalcond[4] =
14087  ((((-1.0) * x685)) +
14088  (((-1.0) * x684)));
14089  evalcond[5] =
14090  ((((-1.0) * x687)) +
14091  x683 + new_r01);
14092  if (IKabs(evalcond[0]) >
14093  IKFAST_EVALCOND_THRESH ||
14094  IKabs(evalcond[1]) >
14095  IKFAST_EVALCOND_THRESH ||
14096  IKabs(evalcond[2]) >
14097  IKFAST_EVALCOND_THRESH ||
14098  IKabs(evalcond[3]) >
14099  IKFAST_EVALCOND_THRESH ||
14100  IKabs(evalcond[4]) >
14101  IKFAST_EVALCOND_THRESH ||
14102  IKabs(evalcond[5]) >
14103  IKFAST_EVALCOND_THRESH)
14104  {
14105  continue;
14106  }
14107  }
14108 
14109  {
14110  std::vector<
14111  IkSingleDOFSolutionBase<
14112  IkReal> >
14113  vinfos(7);
14114  vinfos[0].jointtype = 1;
14115  vinfos[0].foffset = j0;
14116  vinfos[0].indices[0] =
14117  _ij0[0];
14118  vinfos[0].indices[1] =
14119  _ij0[1];
14120  vinfos[0].maxsolutions =
14121  _nj0;
14122  vinfos[1].jointtype = 1;
14123  vinfos[1].foffset = j1;
14124  vinfos[1].indices[0] =
14125  _ij1[0];
14126  vinfos[1].indices[1] =
14127  _ij1[1];
14128  vinfos[1].maxsolutions =
14129  _nj1;
14130  vinfos[2].jointtype = 1;
14131  vinfos[2].foffset = j2;
14132  vinfos[2].indices[0] =
14133  _ij2[0];
14134  vinfos[2].indices[1] =
14135  _ij2[1];
14136  vinfos[2].maxsolutions =
14137  _nj2;
14138  vinfos[3].jointtype = 1;
14139  vinfos[3].foffset = j3;
14140  vinfos[3].indices[0] =
14141  _ij3[0];
14142  vinfos[3].indices[1] =
14143  _ij3[1];
14144  vinfos[3].maxsolutions =
14145  _nj3;
14146  vinfos[4].jointtype = 1;
14147  vinfos[4].foffset = j4;
14148  vinfos[4].indices[0] =
14149  _ij4[0];
14150  vinfos[4].indices[1] =
14151  _ij4[1];
14152  vinfos[4].maxsolutions =
14153  _nj4;
14154  vinfos[5].jointtype = 1;
14155  vinfos[5].foffset = j5;
14156  vinfos[5].indices[0] =
14157  _ij5[0];
14158  vinfos[5].indices[1] =
14159  _ij5[1];
14160  vinfos[5].maxsolutions =
14161  _nj5;
14162  vinfos[6].jointtype = 1;
14163  vinfos[6].foffset = j6;
14164  vinfos[6].indices[0] =
14165  _ij6[0];
14166  vinfos[6].indices[1] =
14167  _ij6[1];
14168  vinfos[6].maxsolutions =
14169  _nj6;
14170  std::vector<int> vfree(
14171  0);
14172  solutions.AddSolution(
14173  vinfos, vfree);
14174  }
14175  }
14176  }
14177  }
14178  }
14179  }
14180  else
14181  {
14182  {
14183  IkReal j0array[1], cj0array[1],
14184  sj0array[1];
14185  bool j0valid[1] = { false };
14186  _nj0 = 1;
14187  CheckValue<IkReal> x688 =
14188  IKPowWithIntegerCheck(
14189  IKsign(
14190  ((((-1.0) *
14191  (gconst11 *
14192  gconst11))) +
14193  (((-1.0) *
14194  (gconst10 *
14195  gconst10))))),
14196  -1);
14197  if (!x688.valid)
14198  {
14199  continue;
14200  }
14201  CheckValue<IkReal> x689 =
14202  IKatan2WithCheck(
14203  IkReal((gconst10 *
14204  new_r11)),
14205  IkReal((gconst11 *
14206  new_r11)),
14207  IKFAST_ATAN2_MAGTHRESH);
14208  if (!x689.valid)
14209  {
14210  continue;
14211  }
14212  j0array[0] =
14213  ((-1.5707963267949) +
14214  (((1.5707963267949) *
14215  (x688.value))) +
14216  (x689.value));
14217  sj0array[0] = IKsin(j0array[0]);
14218  cj0array[0] = IKcos(j0array[0]);
14219  if (j0array[0] > IKPI)
14220  {
14221  j0array[0] -= IK2PI;
14222  }
14223  else if (j0array[0] < -IKPI)
14224  {
14225  j0array[0] += IK2PI;
14226  }
14227  j0valid[0] = true;
14228  for (int ij0 = 0; ij0 < 1;
14229  ++ij0)
14230  {
14231  if (!j0valid[ij0])
14232  {
14233  continue;
14234  }
14235  _ij0[0] = ij0;
14236  _ij0[1] = -1;
14237  for (int iij0 = ij0 + 1;
14238  iij0 < 1;
14239  ++iij0)
14240  {
14241  if (j0valid[iij0] &&
14242  IKabs(cj0array[ij0] -
14243  cj0array[iij0]) <
14244  IKFAST_SOLUTION_THRESH &&
14245  IKabs(sj0array[ij0] -
14246  sj0array[iij0]) <
14247  IKFAST_SOLUTION_THRESH)
14248  {
14249  j0valid[iij0] = false;
14250  _ij0[1] = iij0;
14251  break;
14252  }
14253  }
14254  j0 = j0array[ij0];
14255  cj0 = cj0array[ij0];
14256  sj0 = sj0array[ij0];
14257  {
14258  IkReal evalcond[6];
14259  IkReal x690 = IKsin(j0);
14260  IkReal x691 = IKcos(j0);
14261  IkReal x692 =
14262  (gconst10 * x691);
14263  IkReal x693 =
14264  (gconst11 * x691);
14265  IkReal x694 =
14266  (gconst10 * x690);
14267  IkReal x695 =
14268  ((1.0) * x690);
14269  IkReal x696 =
14270  (gconst11 * x695);
14271  evalcond[0] =
14272  (x692 +
14273  (((-1.0) * x696)));
14274  evalcond[1] =
14275  (gconst10 +
14276  ((new_r11 * x690)) +
14277  ((new_r01 * x691)));
14278  evalcond[2] =
14279  (x694 + x693 + new_r11);
14280  evalcond[3] =
14281  ((((-1.0) * new_r01 *
14282  x695)) +
14283  gconst11 +
14284  ((new_r11 * x691)));
14285  evalcond[4] =
14286  ((((-1.0) * x693)) +
14287  (((-1.0) * x694)));
14288  evalcond[5] =
14289  (x692 +
14290  (((-1.0) * x696)) +
14291  new_r01);
14292  if (IKabs(evalcond[0]) >
14293  IKFAST_EVALCOND_THRESH ||
14294  IKabs(evalcond[1]) >
14295  IKFAST_EVALCOND_THRESH ||
14296  IKabs(evalcond[2]) >
14297  IKFAST_EVALCOND_THRESH ||
14298  IKabs(evalcond[3]) >
14299  IKFAST_EVALCOND_THRESH ||
14300  IKabs(evalcond[4]) >
14301  IKFAST_EVALCOND_THRESH ||
14302  IKabs(evalcond[5]) >
14303  IKFAST_EVALCOND_THRESH)
14304  {
14305  continue;
14306  }
14307  }
14308 
14309  {
14310  std::vector<
14311  IkSingleDOFSolutionBase<
14312  IkReal> >
14313  vinfos(7);
14314  vinfos[0].jointtype = 1;
14315  vinfos[0].foffset = j0;
14316  vinfos[0].indices[0] =
14317  _ij0[0];
14318  vinfos[0].indices[1] =
14319  _ij0[1];
14320  vinfos[0].maxsolutions =
14321  _nj0;
14322  vinfos[1].jointtype = 1;
14323  vinfos[1].foffset = j1;
14324  vinfos[1].indices[0] =
14325  _ij1[0];
14326  vinfos[1].indices[1] =
14327  _ij1[1];
14328  vinfos[1].maxsolutions =
14329  _nj1;
14330  vinfos[2].jointtype = 1;
14331  vinfos[2].foffset = j2;
14332  vinfos[2].indices[0] =
14333  _ij2[0];
14334  vinfos[2].indices[1] =
14335  _ij2[1];
14336  vinfos[2].maxsolutions =
14337  _nj2;
14338  vinfos[3].jointtype = 1;
14339  vinfos[3].foffset = j3;
14340  vinfos[3].indices[0] =
14341  _ij3[0];
14342  vinfos[3].indices[1] =
14343  _ij3[1];
14344  vinfos[3].maxsolutions =
14345  _nj3;
14346  vinfos[4].jointtype = 1;
14347  vinfos[4].foffset = j4;
14348  vinfos[4].indices[0] =
14349  _ij4[0];
14350  vinfos[4].indices[1] =
14351  _ij4[1];
14352  vinfos[4].maxsolutions =
14353  _nj4;
14354  vinfos[5].jointtype = 1;
14355  vinfos[5].foffset = j5;
14356  vinfos[5].indices[0] =
14357  _ij5[0];
14358  vinfos[5].indices[1] =
14359  _ij5[1];
14360  vinfos[5].maxsolutions =
14361  _nj5;
14362  vinfos[6].jointtype = 1;
14363  vinfos[6].foffset = j6;
14364  vinfos[6].indices[0] =
14365  _ij6[0];
14366  vinfos[6].indices[1] =
14367  _ij6[1];
14368  vinfos[6].maxsolutions =
14369  _nj6;
14370  std::vector<int> vfree(0);
14371  solutions.AddSolution(
14372  vinfos, vfree);
14373  }
14374  }
14375  }
14376  }
14377  }
14378  }
14379  else
14380  {
14381  {
14382  IkReal j0array[1], cj0array[1],
14383  sj0array[1];
14384  bool j0valid[1] = { false };
14385  _nj0 = 1;
14386  CheckValue<IkReal> x697 =
14387  IKPowWithIntegerCheck(
14388  IKsign((
14389  ((gconst10 * new_r01)) +
14390  (((-1.0) * gconst11 *
14391  new_r11)))),
14392  -1);
14393  if (!x697.valid)
14394  {
14395  continue;
14396  }
14397  CheckValue<IkReal> x698 =
14398  IKatan2WithCheck(
14399  IkReal(
14400  (gconst10 * gconst11)),
14401  IkReal(gconst11 * gconst11),
14402  IKFAST_ATAN2_MAGTHRESH);
14403  if (!x698.valid)
14404  {
14405  continue;
14406  }
14407  j0array[0] = ((-1.5707963267949) +
14408  (((1.5707963267949) *
14409  (x697.value))) +
14410  (x698.value));
14411  sj0array[0] = IKsin(j0array[0]);
14412  cj0array[0] = IKcos(j0array[0]);
14413  if (j0array[0] > IKPI)
14414  {
14415  j0array[0] -= IK2PI;
14416  }
14417  else if (j0array[0] < -IKPI)
14418  {
14419  j0array[0] += IK2PI;
14420  }
14421  j0valid[0] = true;
14422  for (int ij0 = 0; ij0 < 1; ++ij0)
14423  {
14424  if (!j0valid[ij0])
14425  {
14426  continue;
14427  }
14428  _ij0[0] = ij0;
14429  _ij0[1] = -1;
14430  for (int iij0 = ij0 + 1; iij0 < 1;
14431  ++iij0)
14432  {
14433  if (j0valid[iij0] &&
14434  IKabs(cj0array[ij0] -
14435  cj0array[iij0]) <
14436  IKFAST_SOLUTION_THRESH &&
14437  IKabs(sj0array[ij0] -
14438  sj0array[iij0]) <
14439  IKFAST_SOLUTION_THRESH)
14440  {
14441  j0valid[iij0] = false;
14442  _ij0[1] = iij0;
14443  break;
14444  }
14445  }
14446  j0 = j0array[ij0];
14447  cj0 = cj0array[ij0];
14448  sj0 = sj0array[ij0];
14449  {
14450  IkReal evalcond[6];
14451  IkReal x699 = IKsin(j0);
14452  IkReal x700 = IKcos(j0);
14453  IkReal x701 = (gconst10 * x700);
14454  IkReal x702 = (gconst11 * x700);
14455  IkReal x703 = (gconst10 * x699);
14456  IkReal x704 = ((1.0) * x699);
14457  IkReal x705 = (gconst11 * x704);
14458  evalcond[0] =
14459  ((((-1.0) * x705)) + x701);
14460  evalcond[1] =
14461  (((new_r01 * x700)) +
14462  gconst10 +
14463  ((new_r11 * x699)));
14464  evalcond[2] =
14465  (x702 + x703 + new_r11);
14466  evalcond[3] =
14467  (gconst11 +
14468  ((new_r11 * x700)) +
14469  (((-1.0) * new_r01 *
14470  x704)));
14471  evalcond[4] =
14472  ((((-1.0) * x702)) +
14473  (((-1.0) * x703)));
14474  evalcond[5] =
14475  ((((-1.0) * x705)) + x701 +
14476  new_r01);
14477  if (IKabs(evalcond[0]) >
14478  IKFAST_EVALCOND_THRESH ||
14479  IKabs(evalcond[1]) >
14480  IKFAST_EVALCOND_THRESH ||
14481  IKabs(evalcond[2]) >
14482  IKFAST_EVALCOND_THRESH ||
14483  IKabs(evalcond[3]) >
14484  IKFAST_EVALCOND_THRESH ||
14485  IKabs(evalcond[4]) >
14486  IKFAST_EVALCOND_THRESH ||
14487  IKabs(evalcond[5]) >
14488  IKFAST_EVALCOND_THRESH)
14489  {
14490  continue;
14491  }
14492  }
14493 
14494  {
14495  std::vector<
14496  IkSingleDOFSolutionBase<
14497  IkReal> >
14498  vinfos(7);
14499  vinfos[0].jointtype = 1;
14500  vinfos[0].foffset = j0;
14501  vinfos[0].indices[0] = _ij0[0];
14502  vinfos[0].indices[1] = _ij0[1];
14503  vinfos[0].maxsolutions = _nj0;
14504  vinfos[1].jointtype = 1;
14505  vinfos[1].foffset = j1;
14506  vinfos[1].indices[0] = _ij1[0];
14507  vinfos[1].indices[1] = _ij1[1];
14508  vinfos[1].maxsolutions = _nj1;
14509  vinfos[2].jointtype = 1;
14510  vinfos[2].foffset = j2;
14511  vinfos[2].indices[0] = _ij2[0];
14512  vinfos[2].indices[1] = _ij2[1];
14513  vinfos[2].maxsolutions = _nj2;
14514  vinfos[3].jointtype = 1;
14515  vinfos[3].foffset = j3;
14516  vinfos[3].indices[0] = _ij3[0];
14517  vinfos[3].indices[1] = _ij3[1];
14518  vinfos[3].maxsolutions = _nj3;
14519  vinfos[4].jointtype = 1;
14520  vinfos[4].foffset = j4;
14521  vinfos[4].indices[0] = _ij4[0];
14522  vinfos[4].indices[1] = _ij4[1];
14523  vinfos[4].maxsolutions = _nj4;
14524  vinfos[5].jointtype = 1;
14525  vinfos[5].foffset = j5;
14526  vinfos[5].indices[0] = _ij5[0];
14527  vinfos[5].indices[1] = _ij5[1];
14528  vinfos[5].maxsolutions = _nj5;
14529  vinfos[6].jointtype = 1;
14530  vinfos[6].foffset = j6;
14531  vinfos[6].indices[0] = _ij6[0];
14532  vinfos[6].indices[1] = _ij6[1];
14533  vinfos[6].maxsolutions = _nj6;
14534  std::vector<int> vfree(0);
14535  solutions.AddSolution(vinfos,
14536  vfree);
14537  }
14538  }
14539  }
14540  }
14541  }
14542  }
14543  } while (0);
14544  if (bgotonextstatement)
14545  {
14546  bool bgotonextstatement = true;
14547  do
14548  {
14549  evalcond[0] =
14550  ((IKabs(new_r10)) + (IKabs(new_r01)));
14551  if (IKabs(evalcond[0]) <
14552  0.0000050000000000)
14553  {
14554  bgotonextstatement = false;
14555  {
14556  IkReal j0array[2], cj0array[2],
14557  sj0array[2];
14558  bool j0valid[2] = { false };
14559  _nj0 = 2;
14560  CheckValue<IkReal> x706 =
14561  IKPowWithIntegerCheck(gconst11,
14562  -1);
14563  if (!x706.valid)
14564  {
14565  continue;
14566  }
14567  cj0array[0] =
14568  (new_r00 * (x706.value));
14569  if (cj0array[0] >=
14570  -1 - IKFAST_SINCOS_THRESH &&
14571  cj0array[0] <=
14572  1 + IKFAST_SINCOS_THRESH)
14573  {
14574  j0valid[0] = j0valid[1] = true;
14575  j0array[0] = IKacos(cj0array[0]);
14576  sj0array[0] = IKsin(j0array[0]);
14577  cj0array[1] = cj0array[0];
14578  j0array[1] = -j0array[0];
14579  sj0array[1] = -sj0array[0];
14580  }
14581  else if (isnan(cj0array[0]))
14582  {
14583  // probably any value will work
14584  j0valid[0] = true;
14585  cj0array[0] = 1;
14586  sj0array[0] = 0;
14587  j0array[0] = 0;
14588  }
14589  for (int ij0 = 0; ij0 < 2; ++ij0)
14590  {
14591  if (!j0valid[ij0])
14592  {
14593  continue;
14594  }
14595  _ij0[0] = ij0;
14596  _ij0[1] = -1;
14597  for (int iij0 = ij0 + 1; iij0 < 2;
14598  ++iij0)
14599  {
14600  if (j0valid[iij0] &&
14601  IKabs(cj0array[ij0] -
14602  cj0array[iij0]) <
14603  IKFAST_SOLUTION_THRESH &&
14604  IKabs(sj0array[ij0] -
14605  sj0array[iij0]) <
14606  IKFAST_SOLUTION_THRESH)
14607  {
14608  j0valid[iij0] = false;
14609  _ij0[1] = iij0;
14610  break;
14611  }
14612  }
14613  j0 = j0array[ij0];
14614  cj0 = cj0array[ij0];
14615  sj0 = sj0array[ij0];
14616  {
14617  IkReal evalcond[6];
14618  IkReal x707 = IKsin(j0);
14619  IkReal x708 = IKcos(j0);
14620  IkReal x709 = ((-1.0) * x707);
14621  evalcond[0] = (new_r11 * x707);
14622  evalcond[1] = (new_r00 * x709);
14623  evalcond[2] = (gconst11 * x709);
14624  evalcond[3] =
14625  (gconst11 +
14626  ((new_r11 * x708)));
14627  evalcond[4] =
14628  (new_r11 +
14629  ((gconst11 * x708)));
14630  evalcond[5] =
14631  (((new_r00 * x708)) +
14632  (((-1.0) * gconst11)));
14633  if (IKabs(evalcond[0]) >
14634  IKFAST_EVALCOND_THRESH ||
14635  IKabs(evalcond[1]) >
14636  IKFAST_EVALCOND_THRESH ||
14637  IKabs(evalcond[2]) >
14638  IKFAST_EVALCOND_THRESH ||
14639  IKabs(evalcond[3]) >
14640  IKFAST_EVALCOND_THRESH ||
14641  IKabs(evalcond[4]) >
14642  IKFAST_EVALCOND_THRESH ||
14643  IKabs(evalcond[5]) >
14644  IKFAST_EVALCOND_THRESH)
14645  {
14646  continue;
14647  }
14648  }
14649 
14650  {
14651  std::vector<
14652  IkSingleDOFSolutionBase<
14653  IkReal> >
14654  vinfos(7);
14655  vinfos[0].jointtype = 1;
14656  vinfos[0].foffset = j0;
14657  vinfos[0].indices[0] = _ij0[0];
14658  vinfos[0].indices[1] = _ij0[1];
14659  vinfos[0].maxsolutions = _nj0;
14660  vinfos[1].jointtype = 1;
14661  vinfos[1].foffset = j1;
14662  vinfos[1].indices[0] = _ij1[0];
14663  vinfos[1].indices[1] = _ij1[1];
14664  vinfos[1].maxsolutions = _nj1;
14665  vinfos[2].jointtype = 1;
14666  vinfos[2].foffset = j2;
14667  vinfos[2].indices[0] = _ij2[0];
14668  vinfos[2].indices[1] = _ij2[1];
14669  vinfos[2].maxsolutions = _nj2;
14670  vinfos[3].jointtype = 1;
14671  vinfos[3].foffset = j3;
14672  vinfos[3].indices[0] = _ij3[0];
14673  vinfos[3].indices[1] = _ij3[1];
14674  vinfos[3].maxsolutions = _nj3;
14675  vinfos[4].jointtype = 1;
14676  vinfos[4].foffset = j4;
14677  vinfos[4].indices[0] = _ij4[0];
14678  vinfos[4].indices[1] = _ij4[1];
14679  vinfos[4].maxsolutions = _nj4;
14680  vinfos[5].jointtype = 1;
14681  vinfos[5].foffset = j5;
14682  vinfos[5].indices[0] = _ij5[0];
14683  vinfos[5].indices[1] = _ij5[1];
14684  vinfos[5].maxsolutions = _nj5;
14685  vinfos[6].jointtype = 1;
14686  vinfos[6].foffset = j6;
14687  vinfos[6].indices[0] = _ij6[0];
14688  vinfos[6].indices[1] = _ij6[1];
14689  vinfos[6].maxsolutions = _nj6;
14690  std::vector<int> vfree(0);
14691  solutions.AddSolution(vinfos,
14692  vfree);
14693  }
14694  }
14695  }
14696  }
14697  } while (0);
14698  if (bgotonextstatement)
14699  {
14700  bool bgotonextstatement = true;
14701  do
14702  {
14703  evalcond[0] = ((IKabs(new_r00)) +
14704  (IKabs(new_r01)));
14705  if (IKabs(evalcond[0]) <
14706  0.0000050000000000)
14707  {
14708  bgotonextstatement = false;
14709  {
14710  IkReal j0eval[1];
14711  CheckValue<IkReal> x711 =
14712  IKatan2WithCheck(
14713  IkReal(0),
14714  IkReal(new_r11),
14715  IKFAST_ATAN2_MAGTHRESH);
14716  if (!x711.valid)
14717  {
14718  continue;
14719  }
14720  IkReal x710 =
14721  ((1.0) * (x711.value));
14722  sj1 = 0;
14723  cj1 = -1.0;
14724  j1 = 3.14159265358979;
14725  sj2 = gconst10;
14726  cj2 = gconst11;
14727  j2 = ((3.14159265) +
14728  (((-1.0) * x710)));
14729  new_r00 = 0;
14730  new_r01 = 0;
14731  new_r12 = 0;
14732  new_r22 = 0;
14733  IkReal gconst9 =
14734  ((3.14159265358979) +
14735  (((-1.0) * x710)));
14736  IkReal gconst10 = 0;
14737  IkReal x712 =
14738  ((1.0) +
14739  (((-1.0) *
14740  (new_r10 * new_r10))));
14741  if (IKabs(x712) == 0)
14742  {
14743  continue;
14744  }
14745  IkReal gconst11 =
14746  ((-1.0) * new_r11 *
14747  (pow(x712, -0.5)));
14748  j0eval[0] = ((IKabs(new_r11)) +
14749  (IKabs(new_r10)));
14750  if (IKabs(j0eval[0]) <
14751  0.0000010000000000)
14752  {
14753  {
14754  IkReal j0eval[1];
14755  CheckValue<IkReal> x714 =
14756  IKatan2WithCheck(
14757  IkReal(0),
14758  IkReal(new_r11),
14759  IKFAST_ATAN2_MAGTHRESH);
14760  if (!x714.valid)
14761  {
14762  continue;
14763  }
14764  IkReal x713 =
14765  ((1.0) * (x714.value));
14766  sj1 = 0;
14767  cj1 = -1.0;
14768  j1 = 3.14159265358979;
14769  sj2 = gconst10;
14770  cj2 = gconst11;
14771  j2 = ((3.14159265) +
14772  (((-1.0) * x713)));
14773  new_r00 = 0;
14774  new_r01 = 0;
14775  new_r12 = 0;
14776  new_r22 = 0;
14777  IkReal gconst9 =
14778  ((3.14159265358979) +
14779  (((-1.0) * x713)));
14780  IkReal gconst10 = 0;
14781  IkReal x715 =
14782  ((1.0) +
14783  (((-1.0) *
14784  (new_r10 * new_r10))));
14785  if (IKabs(x715) == 0)
14786  {
14787  continue;
14788  }
14789  IkReal gconst11 =
14790  ((-1.0) * new_r11 *
14791  (pow(x715, -0.5)));
14792  j0eval[0] = new_r11;
14793  if (IKabs(j0eval[0]) <
14794  0.0000010000000000)
14795  {
14796  {
14797  IkReal j0eval[2];
14798  CheckValue<IkReal> x717 =
14799  IKatan2WithCheck(
14800  IkReal(0),
14801  IkReal(new_r11),
14802  IKFAST_ATAN2_MAGTHRESH);
14803  if (!x717.valid)
14804  {
14805  continue;
14806  }
14807  IkReal x716 =
14808  ((1.0) * (x717.value));
14809  sj1 = 0;
14810  cj1 = -1.0;
14811  j1 = 3.14159265358979;
14812  sj2 = gconst10;
14813  cj2 = gconst11;
14814  j2 = ((3.14159265) +
14815  (((-1.0) * x716)));
14816  new_r00 = 0;
14817  new_r01 = 0;
14818  new_r12 = 0;
14819  new_r22 = 0;
14820  IkReal gconst9 =
14821  ((3.14159265358979) +
14822  (((-1.0) * x716)));
14823  IkReal gconst10 = 0;
14824  IkReal x718 =
14825  ((1.0) + (((-1.0) *
14826  (new_r10 *
14827  new_r10))));
14828  if (IKabs(x718) == 0)
14829  {
14830  continue;
14831  }
14832  IkReal gconst11 =
14833  ((-1.0) * new_r11 *
14834  (pow(x718, -0.5)));
14835  j0eval[0] = new_r10;
14836  j0eval[1] = new_r11;
14837  if (IKabs(j0eval[0]) <
14838  0.0000010000000000 ||
14839  IKabs(j0eval[1]) <
14840  0.0000010000000000)
14841  {
14842  continue; // 3 cases
14843  // reached
14844  }
14845  else
14846  {
14847  {
14848  IkReal j0array[1],
14849  cj0array[1],
14850  sj0array[1];
14851  bool j0valid[1] = {
14852  false
14853  };
14854  _nj0 = 1;
14855  CheckValue<IkReal> x719 =
14856  IKPowWithIntegerCheck(
14857  new_r10, -1);
14858  if (!x719.valid)
14859  {
14860  continue;
14861  }
14862  CheckValue<IkReal> x720 =
14863  IKPowWithIntegerCheck(
14864  new_r11, -1);
14865  if (!x720.valid)
14866  {
14867  continue;
14868  }
14869  if (IKabs((
14870  gconst11 *
14871  (x719.value))) <
14872  IKFAST_ATAN2_MAGTHRESH &&
14873  IKabs((
14874  (-1.0) *
14875  gconst11 *
14876  (x720.value))) <
14877  IKFAST_ATAN2_MAGTHRESH &&
14878  IKabs(
14879  IKsqr((
14880  gconst11 *
14881  (x719.value))) +
14882  IKsqr((
14883  (-1.0) *
14884  gconst11 *
14885  (x720.value))) -
14886  1) <=
14887  IKFAST_SINCOS_THRESH)
14888  continue;
14889  j0array[0] = IKatan2(
14890  (gconst11 *
14891  (x719.value)),
14892  ((-1.0) * gconst11 *
14893  (x720.value)));
14894  sj0array[0] =
14895  IKsin(j0array[0]);
14896  cj0array[0] =
14897  IKcos(j0array[0]);
14898  if (j0array[0] > IKPI)
14899  {
14900  j0array[0] -= IK2PI;
14901  }
14902  else if (j0array[0] <
14903  -IKPI)
14904  {
14905  j0array[0] += IK2PI;
14906  }
14907  j0valid[0] = true;
14908  for (int ij0 = 0;
14909  ij0 < 1;
14910  ++ij0)
14911  {
14912  if (!j0valid[ij0])
14913  {
14914  continue;
14915  }
14916  _ij0[0] = ij0;
14917  _ij0[1] = -1;
14918  for (int iij0 =
14919  ij0 + 1;
14920  iij0 < 1;
14921  ++iij0)
14922  {
14923  if (j0valid[iij0] &&
14924  IKabs(
14925  cj0array
14926  [ij0] -
14927  cj0array
14928  [iij0]) <
14929  IKFAST_SOLUTION_THRESH &&
14930  IKabs(
14931  sj0array
14932  [ij0] -
14933  sj0array
14934  [iij0]) <
14935  IKFAST_SOLUTION_THRESH)
14936  {
14937  j0valid[iij0] =
14938  false;
14939  _ij0[1] = iij0;
14940  break;
14941  }
14942  }
14943  j0 = j0array[ij0];
14944  cj0 = cj0array[ij0];
14945  sj0 = sj0array[ij0];
14946  {
14947  IkReal evalcond[8];
14948  IkReal x721 =
14949  IKcos(j0);
14950  IkReal x722 =
14951  IKsin(j0);
14952  IkReal x723 =
14953  ((1.0) *
14954  gconst11);
14955  IkReal x724 =
14956  ((-1.0) *
14957  gconst11);
14958  evalcond[0] =
14959  (new_r10 *
14960  x721);
14961  evalcond[1] =
14962  (new_r11 *
14963  x722);
14964  evalcond[2] =
14965  (x721 * x724);
14966  evalcond[3] =
14967  (x722 * x724);
14968  evalcond[4] =
14969  (gconst11 +
14970  ((new_r11 *
14971  x721)));
14972  evalcond[5] =
14973  (((gconst11 *
14974  x721)) +
14975  new_r11);
14976  evalcond[6] =
14977  ((((-1.0) *
14978  x722 *
14979  x723)) +
14980  new_r10);
14981  evalcond[7] =
14982  ((((-1.0) *
14983  x723)) +
14984  ((new_r10 *
14985  x722)));
14986  if (IKabs(evalcond
14987  [0]) >
14988  IKFAST_EVALCOND_THRESH ||
14989  IKabs(evalcond
14990  [1]) >
14991  IKFAST_EVALCOND_THRESH ||
14992  IKabs(evalcond
14993  [2]) >
14994  IKFAST_EVALCOND_THRESH ||
14995  IKabs(evalcond
14996  [3]) >
14997  IKFAST_EVALCOND_THRESH ||
14998  IKabs(evalcond
14999  [4]) >
15000  IKFAST_EVALCOND_THRESH ||
15001  IKabs(evalcond
15002  [5]) >
15003  IKFAST_EVALCOND_THRESH ||
15004  IKabs(evalcond
15005  [6]) >
15006  IKFAST_EVALCOND_THRESH ||
15007  IKabs(evalcond
15008  [7]) >
15009  IKFAST_EVALCOND_THRESH)
15010  {
15011  continue;
15012  }
15013  }
15014 
15015  {
15016  std::vector<
15017  IkSingleDOFSolutionBase<
15018  IkReal> >
15019  vinfos(7);
15020  vinfos[0]
15021  .jointtype = 1;
15022  vinfos[0].foffset =
15023  j0;
15024  vinfos[0]
15025  .indices[0] =
15026  _ij0[0];
15027  vinfos[0]
15028  .indices[1] =
15029  _ij0[1];
15030  vinfos[0]
15031  .maxsolutions =
15032  _nj0;
15033  vinfos[1]
15034  .jointtype = 1;
15035  vinfos[1].foffset =
15036  j1;
15037  vinfos[1]
15038  .indices[0] =
15039  _ij1[0];
15040  vinfos[1]
15041  .indices[1] =
15042  _ij1[1];
15043  vinfos[1]
15044  .maxsolutions =
15045  _nj1;
15046  vinfos[2]
15047  .jointtype = 1;
15048  vinfos[2].foffset =
15049  j2;
15050  vinfos[2]
15051  .indices[0] =
15052  _ij2[0];
15053  vinfos[2]
15054  .indices[1] =
15055  _ij2[1];
15056  vinfos[2]
15057  .maxsolutions =
15058  _nj2;
15059  vinfos[3]
15060  .jointtype = 1;
15061  vinfos[3].foffset =
15062  j3;
15063  vinfos[3]
15064  .indices[0] =
15065  _ij3[0];
15066  vinfos[3]
15067  .indices[1] =
15068  _ij3[1];
15069  vinfos[3]
15070  .maxsolutions =
15071  _nj3;
15072  vinfos[4]
15073  .jointtype = 1;
15074  vinfos[4].foffset =
15075  j4;
15076  vinfos[4]
15077  .indices[0] =
15078  _ij4[0];
15079  vinfos[4]
15080  .indices[1] =
15081  _ij4[1];
15082  vinfos[4]
15083  .maxsolutions =
15084  _nj4;
15085  vinfos[5]
15086  .jointtype = 1;
15087  vinfos[5].foffset =
15088  j5;
15089  vinfos[5]
15090  .indices[0] =
15091  _ij5[0];
15092  vinfos[5]
15093  .indices[1] =
15094  _ij5[1];
15095  vinfos[5]
15096  .maxsolutions =
15097  _nj5;
15098  vinfos[6]
15099  .jointtype = 1;
15100  vinfos[6].foffset =
15101  j6;
15102  vinfos[6]
15103  .indices[0] =
15104  _ij6[0];
15105  vinfos[6]
15106  .indices[1] =
15107  _ij6[1];
15108  vinfos[6]
15109  .maxsolutions =
15110  _nj6;
15111  std::vector<int>
15112  vfree(0);
15113  solutions
15114  .AddSolution(
15115  vinfos,
15116  vfree);
15117  }
15118  }
15119  }
15120  }
15121  }
15122  }
15123  else
15124  {
15125  {
15126  IkReal j0array[1],
15127  cj0array[1],
15128  sj0array[1];
15129  bool j0valid[1] = { false };
15130  _nj0 = 1;
15131  CheckValue<IkReal> x725 =
15132  IKPowWithIntegerCheck(
15133  gconst11, -1);
15134  if (!x725.valid)
15135  {
15136  continue;
15137  }
15138  CheckValue<IkReal> x726 =
15139  IKPowWithIntegerCheck(
15140  new_r11, -1);
15141  if (!x726.valid)
15142  {
15143  continue;
15144  }
15145  if (IKabs((new_r10 *
15146  (x725.value))) <
15147  IKFAST_ATAN2_MAGTHRESH &&
15148  IKabs(
15149  ((-1.0) * gconst11 *
15150  (x726.value))) <
15151  IKFAST_ATAN2_MAGTHRESH &&
15152  IKabs(
15153  IKsqr((
15154  new_r10 *
15155  (x725.value))) +
15156  IKsqr((
15157  (-1.0) *
15158  gconst11 *
15159  (x726.value))) -
15160  1) <=
15161  IKFAST_SINCOS_THRESH)
15162  continue;
15163  j0array[0] = IKatan2(
15164  (new_r10 *
15165  (x725.value)),
15166  ((-1.0) * gconst11 *
15167  (x726.value)));
15168  sj0array[0] =
15169  IKsin(j0array[0]);
15170  cj0array[0] =
15171  IKcos(j0array[0]);
15172  if (j0array[0] > IKPI)
15173  {
15174  j0array[0] -= IK2PI;
15175  }
15176  else if (j0array[0] < -IKPI)
15177  {
15178  j0array[0] += IK2PI;
15179  }
15180  j0valid[0] = true;
15181  for (int ij0 = 0; ij0 < 1;
15182  ++ij0)
15183  {
15184  if (!j0valid[ij0])
15185  {
15186  continue;
15187  }
15188  _ij0[0] = ij0;
15189  _ij0[1] = -1;
15190  for (int iij0 = ij0 + 1;
15191  iij0 < 1;
15192  ++iij0)
15193  {
15194  if (j0valid[iij0] &&
15195  IKabs(
15196  cj0array[ij0] -
15197  cj0array
15198  [iij0]) <
15199  IKFAST_SOLUTION_THRESH &&
15200  IKabs(
15201  sj0array[ij0] -
15202  sj0array
15203  [iij0]) <
15204  IKFAST_SOLUTION_THRESH)
15205  {
15206  j0valid[iij0] = false;
15207  _ij0[1] = iij0;
15208  break;
15209  }
15210  }
15211  j0 = j0array[ij0];
15212  cj0 = cj0array[ij0];
15213  sj0 = sj0array[ij0];
15214  {
15215  IkReal evalcond[8];
15216  IkReal x727 = IKcos(j0);
15217  IkReal x728 = IKsin(j0);
15218  IkReal x729 =
15219  ((1.0) * gconst11);
15220  IkReal x730 =
15221  ((-1.0) * gconst11);
15222  evalcond[0] =
15223  (new_r10 * x727);
15224  evalcond[1] =
15225  (new_r11 * x728);
15226  evalcond[2] =
15227  (x727 * x730);
15228  evalcond[3] =
15229  (x728 * x730);
15230  evalcond[4] =
15231  (gconst11 +
15232  ((new_r11 *
15233  x727)));
15234  evalcond[5] =
15235  (((gconst11 *
15236  x727)) +
15237  new_r11);
15238  evalcond[6] =
15239  (new_r10 +
15240  (((-1.0) * x728 *
15241  x729)));
15242  evalcond[7] =
15243  ((((-1.0) * x729)) +
15244  ((new_r10 *
15245  x728)));
15246  if (IKabs(evalcond[0]) >
15247  IKFAST_EVALCOND_THRESH ||
15248  IKabs(evalcond[1]) >
15249  IKFAST_EVALCOND_THRESH ||
15250  IKabs(evalcond[2]) >
15251  IKFAST_EVALCOND_THRESH ||
15252  IKabs(evalcond[3]) >
15253  IKFAST_EVALCOND_THRESH ||
15254  IKabs(evalcond[4]) >
15255  IKFAST_EVALCOND_THRESH ||
15256  IKabs(evalcond[5]) >
15257  IKFAST_EVALCOND_THRESH ||
15258  IKabs(evalcond[6]) >
15259  IKFAST_EVALCOND_THRESH ||
15260  IKabs(evalcond[7]) >
15261  IKFAST_EVALCOND_THRESH)
15262  {
15263  continue;
15264  }
15265  }
15266 
15267  {
15268  std::vector<
15269  IkSingleDOFSolutionBase<
15270  IkReal> >
15271  vinfos(7);
15272  vinfos[0].jointtype = 1;
15273  vinfos[0].foffset = j0;
15274  vinfos[0].indices[0] =
15275  _ij0[0];
15276  vinfos[0].indices[1] =
15277  _ij0[1];
15278  vinfos[0].maxsolutions =
15279  _nj0;
15280  vinfos[1].jointtype = 1;
15281  vinfos[1].foffset = j1;
15282  vinfos[1].indices[0] =
15283  _ij1[0];
15284  vinfos[1].indices[1] =
15285  _ij1[1];
15286  vinfos[1].maxsolutions =
15287  _nj1;
15288  vinfos[2].jointtype = 1;
15289  vinfos[2].foffset = j2;
15290  vinfos[2].indices[0] =
15291  _ij2[0];
15292  vinfos[2].indices[1] =
15293  _ij2[1];
15294  vinfos[2].maxsolutions =
15295  _nj2;
15296  vinfos[3].jointtype = 1;
15297  vinfos[3].foffset = j3;
15298  vinfos[3].indices[0] =
15299  _ij3[0];
15300  vinfos[3].indices[1] =
15301  _ij3[1];
15302  vinfos[3].maxsolutions =
15303  _nj3;
15304  vinfos[4].jointtype = 1;
15305  vinfos[4].foffset = j4;
15306  vinfos[4].indices[0] =
15307  _ij4[0];
15308  vinfos[4].indices[1] =
15309  _ij4[1];
15310  vinfos[4].maxsolutions =
15311  _nj4;
15312  vinfos[5].jointtype = 1;
15313  vinfos[5].foffset = j5;
15314  vinfos[5].indices[0] =
15315  _ij5[0];
15316  vinfos[5].indices[1] =
15317  _ij5[1];
15318  vinfos[5].maxsolutions =
15319  _nj5;
15320  vinfos[6].jointtype = 1;
15321  vinfos[6].foffset = j6;
15322  vinfos[6].indices[0] =
15323  _ij6[0];
15324  vinfos[6].indices[1] =
15325  _ij6[1];
15326  vinfos[6].maxsolutions =
15327  _nj6;
15328  std::vector<int> vfree(
15329  0);
15330  solutions.AddSolution(
15331  vinfos, vfree);
15332  }
15333  }
15334  }
15335  }
15336  }
15337  }
15338  else
15339  {
15340  {
15341  IkReal j0array[1], cj0array[1],
15342  sj0array[1];
15343  bool j0valid[1] = { false };
15344  _nj0 = 1;
15345  CheckValue<IkReal> x731 =
15346  IKPowWithIntegerCheck(
15347  IKsign(gconst11), -1);
15348  if (!x731.valid)
15349  {
15350  continue;
15351  }
15352  CheckValue<IkReal> x732 =
15353  IKatan2WithCheck(
15354  IkReal(new_r10),
15355  IkReal(
15356  ((-1.0) * new_r11)),
15357  IKFAST_ATAN2_MAGTHRESH);
15358  if (!x732.valid)
15359  {
15360  continue;
15361  }
15362  j0array[0] =
15363  ((-1.5707963267949) +
15364  (((1.5707963267949) *
15365  (x731.value))) +
15366  (x732.value));
15367  sj0array[0] = IKsin(j0array[0]);
15368  cj0array[0] = IKcos(j0array[0]);
15369  if (j0array[0] > IKPI)
15370  {
15371  j0array[0] -= IK2PI;
15372  }
15373  else if (j0array[0] < -IKPI)
15374  {
15375  j0array[0] += IK2PI;
15376  }
15377  j0valid[0] = true;
15378  for (int ij0 = 0; ij0 < 1;
15379  ++ij0)
15380  {
15381  if (!j0valid[ij0])
15382  {
15383  continue;
15384  }
15385  _ij0[0] = ij0;
15386  _ij0[1] = -1;
15387  for (int iij0 = ij0 + 1;
15388  iij0 < 1;
15389  ++iij0)
15390  {
15391  if (j0valid[iij0] &&
15392  IKabs(cj0array[ij0] -
15393  cj0array[iij0]) <
15394  IKFAST_SOLUTION_THRESH &&
15395  IKabs(sj0array[ij0] -
15396  sj0array[iij0]) <
15397  IKFAST_SOLUTION_THRESH)
15398  {
15399  j0valid[iij0] = false;
15400  _ij0[1] = iij0;
15401  break;
15402  }
15403  }
15404  j0 = j0array[ij0];
15405  cj0 = cj0array[ij0];
15406  sj0 = sj0array[ij0];
15407  {
15408  IkReal evalcond[8];
15409  IkReal x733 = IKcos(j0);
15410  IkReal x734 = IKsin(j0);
15411  IkReal x735 =
15412  ((1.0) * gconst11);
15413  IkReal x736 =
15414  ((-1.0) * gconst11);
15415  evalcond[0] =
15416  (new_r10 * x733);
15417  evalcond[1] =
15418  (new_r11 * x734);
15419  evalcond[2] = (x733 * x736);
15420  evalcond[3] = (x734 * x736);
15421  evalcond[4] =
15422  (gconst11 +
15423  ((new_r11 * x733)));
15424  evalcond[5] =
15425  (new_r11 +
15426  ((gconst11 * x733)));
15427  evalcond[6] =
15428  ((((-1.0) * x734 *
15429  x735)) +
15430  new_r10);
15431  evalcond[7] =
15432  ((((-1.0) * x735)) +
15433  ((new_r10 * x734)));
15434  if (IKabs(evalcond[0]) >
15435  IKFAST_EVALCOND_THRESH ||
15436  IKabs(evalcond[1]) >
15437  IKFAST_EVALCOND_THRESH ||
15438  IKabs(evalcond[2]) >
15439  IKFAST_EVALCOND_THRESH ||
15440  IKabs(evalcond[3]) >
15441  IKFAST_EVALCOND_THRESH ||
15442  IKabs(evalcond[4]) >
15443  IKFAST_EVALCOND_THRESH ||
15444  IKabs(evalcond[5]) >
15445  IKFAST_EVALCOND_THRESH ||
15446  IKabs(evalcond[6]) >
15447  IKFAST_EVALCOND_THRESH ||
15448  IKabs(evalcond[7]) >
15449  IKFAST_EVALCOND_THRESH)
15450  {
15451  continue;
15452  }
15453  }
15454 
15455  {
15456  std::vector<
15457  IkSingleDOFSolutionBase<
15458  IkReal> >
15459  vinfos(7);
15460  vinfos[0].jointtype = 1;
15461  vinfos[0].foffset = j0;
15462  vinfos[0].indices[0] =
15463  _ij0[0];
15464  vinfos[0].indices[1] =
15465  _ij0[1];
15466  vinfos[0].maxsolutions =
15467  _nj0;
15468  vinfos[1].jointtype = 1;
15469  vinfos[1].foffset = j1;
15470  vinfos[1].indices[0] =
15471  _ij1[0];
15472  vinfos[1].indices[1] =
15473  _ij1[1];
15474  vinfos[1].maxsolutions =
15475  _nj1;
15476  vinfos[2].jointtype = 1;
15477  vinfos[2].foffset = j2;
15478  vinfos[2].indices[0] =
15479  _ij2[0];
15480  vinfos[2].indices[1] =
15481  _ij2[1];
15482  vinfos[2].maxsolutions =
15483  _nj2;
15484  vinfos[3].jointtype = 1;
15485  vinfos[3].foffset = j3;
15486  vinfos[3].indices[0] =
15487  _ij3[0];
15488  vinfos[3].indices[1] =
15489  _ij3[1];
15490  vinfos[3].maxsolutions =
15491  _nj3;
15492  vinfos[4].jointtype = 1;
15493  vinfos[4].foffset = j4;
15494  vinfos[4].indices[0] =
15495  _ij4[0];
15496  vinfos[4].indices[1] =
15497  _ij4[1];
15498  vinfos[4].maxsolutions =
15499  _nj4;
15500  vinfos[5].jointtype = 1;
15501  vinfos[5].foffset = j5;
15502  vinfos[5].indices[0] =
15503  _ij5[0];
15504  vinfos[5].indices[1] =
15505  _ij5[1];
15506  vinfos[5].maxsolutions =
15507  _nj5;
15508  vinfos[6].jointtype = 1;
15509  vinfos[6].foffset = j6;
15510  vinfos[6].indices[0] =
15511  _ij6[0];
15512  vinfos[6].indices[1] =
15513  _ij6[1];
15514  vinfos[6].maxsolutions =
15515  _nj6;
15516  std::vector<int> vfree(0);
15517  solutions.AddSolution(
15518  vinfos, vfree);
15519  }
15520  }
15521  }
15522  }
15523  }
15524  }
15525  } while (0);
15526  if (bgotonextstatement)
15527  {
15528  bool bgotonextstatement = true;
15529  do
15530  {
15531  evalcond[0] = IKabs(new_r01);
15532  if (IKabs(evalcond[0]) <
15533  0.0000050000000000)
15534  {
15535  bgotonextstatement = false;
15536  {
15537  IkReal j0eval[1];
15538  CheckValue<IkReal> x738 =
15539  IKatan2WithCheck(
15540  IkReal(0),
15541  IkReal(new_r11),
15542  IKFAST_ATAN2_MAGTHRESH);
15543  if (!x738.valid)
15544  {
15545  continue;
15546  }
15547  IkReal x737 =
15548  ((1.0) * (x738.value));
15549  sj1 = 0;
15550  cj1 = -1.0;
15551  j1 = 3.14159265358979;
15552  sj2 = gconst10;
15553  cj2 = gconst11;
15554  j2 = ((3.14159265) +
15555  (((-1.0) * x737)));
15556  new_r01 = 0;
15557  IkReal gconst9 =
15558  ((3.14159265358979) +
15559  (((-1.0) * x737)));
15560  IkReal gconst10 = 0;
15561  IkReal x739 = new_r11 * new_r11;
15562  if (IKabs(x739) == 0)
15563  {
15564  continue;
15565  }
15566  IkReal gconst11 =
15567  ((-1.0) * new_r11 *
15568  (pow(x739, -0.5)));
15569  j0eval[0] = ((IKabs(new_r10)) +
15570  (IKabs(new_r00)));
15571  if (IKabs(j0eval[0]) <
15572  0.0000010000000000)
15573  {
15574  {
15575  IkReal j0eval[1];
15576  CheckValue<IkReal> x741 =
15577  IKatan2WithCheck(
15578  IkReal(0),
15579  IkReal(new_r11),
15580  IKFAST_ATAN2_MAGTHRESH);
15581  if (!x741.valid)
15582  {
15583  continue;
15584  }
15585  IkReal x740 =
15586  ((1.0) * (x741.value));
15587  sj1 = 0;
15588  cj1 = -1.0;
15589  j1 = 3.14159265358979;
15590  sj2 = gconst10;
15591  cj2 = gconst11;
15592  j2 = ((3.14159265) +
15593  (((-1.0) * x740)));
15594  new_r01 = 0;
15595  IkReal gconst9 =
15596  ((3.14159265358979) +
15597  (((-1.0) * x740)));
15598  IkReal gconst10 = 0;
15599  IkReal x742 =
15600  new_r11 * new_r11;
15601  if (IKabs(x742) == 0)
15602  {
15603  continue;
15604  }
15605  IkReal gconst11 =
15606  ((-1.0) * new_r11 *
15607  (pow(x742, -0.5)));
15608  j0eval[0] =
15609  ((IKabs(new_r11)) +
15610  (IKabs(new_r10)));
15611  if (IKabs(j0eval[0]) <
15612  0.0000010000000000)
15613  {
15614  {
15615  IkReal j0eval[1];
15616  CheckValue<IkReal> x744 =
15617  IKatan2WithCheck(
15618  IkReal(0),
15619  IkReal(new_r11),
15620  IKFAST_ATAN2_MAGTHRESH);
15621  if (!x744.valid)
15622  {
15623  continue;
15624  }
15625  IkReal x743 =
15626  ((1.0) *
15627  (x744.value));
15628  sj1 = 0;
15629  cj1 = -1.0;
15630  j1 = 3.14159265358979;
15631  sj2 = gconst10;
15632  cj2 = gconst11;
15633  j2 = ((3.14159265) +
15634  (((-1.0) * x743)));
15635  new_r01 = 0;
15636  IkReal gconst9 =
15637  ((3.14159265358979) +
15638  (((-1.0) * x743)));
15639  IkReal gconst10 = 0;
15640  IkReal x745 =
15641  new_r11 * new_r11;
15642  if (IKabs(x745) == 0)
15643  {
15644  continue;
15645  }
15646  IkReal gconst11 =
15647  ((-1.0) * new_r11 *
15648  (pow(x745, -0.5)));
15649  j0eval[0] = new_r11;
15650  if (IKabs(j0eval[0]) <
15651  0.0000010000000000)
15652  {
15653  continue; // 3 cases
15654  // reached
15655  }
15656  else
15657  {
15658  {
15659  IkReal j0array[1],
15660  cj0array[1],
15661  sj0array[1];
15662  bool j0valid[1] = {
15663  false
15664  };
15665  _nj0 = 1;
15666  CheckValue<IkReal> x746 =
15667  IKPowWithIntegerCheck(
15668  gconst11, -1);
15669  if (!x746.valid)
15670  {
15671  continue;
15672  }
15673  CheckValue<IkReal> x747 =
15674  IKPowWithIntegerCheck(
15675  new_r11, -1);
15676  if (!x747.valid)
15677  {
15678  continue;
15679  }
15680  if (IKabs((
15681  new_r10 *
15682  (x746.value))) <
15683  IKFAST_ATAN2_MAGTHRESH &&
15684  IKabs((
15685  (-1.0) *
15686  gconst11 *
15687  (x747.value))) <
15688  IKFAST_ATAN2_MAGTHRESH &&
15689  IKabs(
15690  IKsqr((
15691  new_r10 *
15692  (x746.value))) +
15693  IKsqr((
15694  (-1.0) *
15695  gconst11 *
15696  (x747.value))) -
15697  1) <=
15698  IKFAST_SINCOS_THRESH)
15699  continue;
15700  j0array[0] = IKatan2(
15701  (new_r10 *
15702  (x746.value)),
15703  ((-1.0) *
15704  gconst11 *
15705  (x747.value)));
15706  sj0array[0] =
15707  IKsin(j0array[0]);
15708  cj0array[0] =
15709  IKcos(j0array[0]);
15710  if (j0array[0] > IKPI)
15711  {
15712  j0array[0] -= IK2PI;
15713  }
15714  else if (j0array[0] <
15715  -IKPI)
15716  {
15717  j0array[0] += IK2PI;
15718  }
15719  j0valid[0] = true;
15720  for (int ij0 = 0;
15721  ij0 < 1;
15722  ++ij0)
15723  {
15724  if (!j0valid[ij0])
15725  {
15726  continue;
15727  }
15728  _ij0[0] = ij0;
15729  _ij0[1] = -1;
15730  for (int iij0 =
15731  ij0 + 1;
15732  iij0 < 1;
15733  ++iij0)
15734  {
15735  if (j0valid
15736  [iij0] &&
15737  IKabs(
15738  cj0array
15739  [ij0] -
15740  cj0array
15741  [iij0]) <
15742  IKFAST_SOLUTION_THRESH &&
15743  IKabs(
15744  sj0array
15745  [ij0] -
15746  sj0array
15747  [iij0]) <
15748  IKFAST_SOLUTION_THRESH)
15749  {
15750  j0valid[iij0] =
15751  false;
15752  _ij0[1] = iij0;
15753  break;
15754  }
15755  }
15756  j0 = j0array[ij0];
15757  cj0 = cj0array[ij0];
15758  sj0 = sj0array[ij0];
15759  {
15760  IkReal
15761  evalcond[8];
15762  IkReal x748 =
15763  IKsin(j0);
15764  IkReal x749 =
15765  IKcos(j0);
15766  IkReal x750 =
15767  (gconst11 *
15768  x748);
15769  IkReal x751 =
15770  (gconst11 *
15771  x749);
15772  evalcond[0] =
15773  (new_r11 *
15774  x748);
15775  evalcond[1] =
15776  ((-1.0) *
15777  x750);
15778  evalcond[2] =
15779  (gconst11 +
15780  ((new_r11 *
15781  x749)));
15782  evalcond[3] =
15783  (x751 +
15784  new_r11);
15785  evalcond[4] =
15786  ((((-1.0) *
15787  x750)) +
15788  new_r10);
15789  evalcond[5] =
15790  ((((-1.0) *
15791  x751)) +
15792  new_r00);
15793  evalcond[6] =
15794  ((((-1.0) *
15795  new_r00 *
15796  x748)) +
15797  ((new_r10 *
15798  x749)));
15799  evalcond[7] =
15800  (((new_r10 *
15801  x748)) +
15802  ((new_r00 *
15803  x749)) +
15804  (((-1.0) *
15805  gconst11)));
15806  if (IKabs(
15807  evalcond
15808  [0]) >
15809  IKFAST_EVALCOND_THRESH ||
15810  IKabs(
15811  evalcond
15812  [1]) >
15813  IKFAST_EVALCOND_THRESH ||
15814  IKabs(
15815  evalcond
15816  [2]) >
15817  IKFAST_EVALCOND_THRESH ||
15818  IKabs(
15819  evalcond
15820  [3]) >
15821  IKFAST_EVALCOND_THRESH ||
15822  IKabs(
15823  evalcond
15824  [4]) >
15825  IKFAST_EVALCOND_THRESH ||
15826  IKabs(
15827  evalcond
15828  [5]) >
15829  IKFAST_EVALCOND_THRESH ||
15830  IKabs(
15831  evalcond
15832  [6]) >
15833  IKFAST_EVALCOND_THRESH ||
15834  IKabs(
15835  evalcond
15836  [7]) >
15837  IKFAST_EVALCOND_THRESH)
15838  {
15839  continue;
15840  }
15841  }
15842 
15843  {
15844  std::vector<
15845  IkSingleDOFSolutionBase<
15846  IkReal> >
15847  vinfos(7);
15848  vinfos[0]
15849  .jointtype =
15850  1;
15851  vinfos[0]
15852  .foffset = j0;
15853  vinfos[0]
15854  .indices[0] =
15855  _ij0[0];
15856  vinfos[0]
15857  .indices[1] =
15858  _ij0[1];
15859  vinfos[0]
15860  .maxsolutions =
15861  _nj0;
15862  vinfos[1]
15863  .jointtype =
15864  1;
15865  vinfos[1]
15866  .foffset = j1;
15867  vinfos[1]
15868  .indices[0] =
15869  _ij1[0];
15870  vinfos[1]
15871  .indices[1] =
15872  _ij1[1];
15873  vinfos[1]
15874  .maxsolutions =
15875  _nj1;
15876  vinfos[2]
15877  .jointtype =
15878  1;
15879  vinfos[2]
15880  .foffset = j2;
15881  vinfos[2]
15882  .indices[0] =
15883  _ij2[0];
15884  vinfos[2]
15885  .indices[1] =
15886  _ij2[1];
15887  vinfos[2]
15888  .maxsolutions =
15889  _nj2;
15890  vinfos[3]
15891  .jointtype =
15892  1;
15893  vinfos[3]
15894  .foffset = j3;
15895  vinfos[3]
15896  .indices[0] =
15897  _ij3[0];
15898  vinfos[3]
15899  .indices[1] =
15900  _ij3[1];
15901  vinfos[3]
15902  .maxsolutions =
15903  _nj3;
15904  vinfos[4]
15905  .jointtype =
15906  1;
15907  vinfos[4]
15908  .foffset = j4;
15909  vinfos[4]
15910  .indices[0] =
15911  _ij4[0];
15912  vinfos[4]
15913  .indices[1] =
15914  _ij4[1];
15915  vinfos[4]
15916  .maxsolutions =
15917  _nj4;
15918  vinfos[5]
15919  .jointtype =
15920  1;
15921  vinfos[5]
15922  .foffset = j5;
15923  vinfos[5]
15924  .indices[0] =
15925  _ij5[0];
15926  vinfos[5]
15927  .indices[1] =
15928  _ij5[1];
15929  vinfos[5]
15930  .maxsolutions =
15931  _nj5;
15932  vinfos[6]
15933  .jointtype =
15934  1;
15935  vinfos[6]
15936  .foffset = j6;
15937  vinfos[6]
15938  .indices[0] =
15939  _ij6[0];
15940  vinfos[6]
15941  .indices[1] =
15942  _ij6[1];
15943  vinfos[6]
15944  .maxsolutions =
15945  _nj6;
15946  std::vector<int>
15947  vfree(0);
15948  solutions
15949  .AddSolution(
15950  vinfos,
15951  vfree);
15952  }
15953  }
15954  }
15955  }
15956  }
15957  }
15958  else
15959  {
15960  {
15961  IkReal j0array[1],
15962  cj0array[1],
15963  sj0array[1];
15964  bool j0valid[1] = {
15965  false
15966  };
15967  _nj0 = 1;
15968  CheckValue<IkReal> x752 =
15969  IKPowWithIntegerCheck(
15970  IKsign(gconst11),
15971  -1);
15972  if (!x752.valid)
15973  {
15974  continue;
15975  }
15976  CheckValue<IkReal> x753 =
15977  IKatan2WithCheck(
15978  IkReal(new_r10),
15979  IkReal(((-1.0) *
15980  new_r11)),
15981  IKFAST_ATAN2_MAGTHRESH);
15982  if (!x753.valid)
15983  {
15984  continue;
15985  }
15986  j0array[0] =
15987  ((-1.5707963267949) +
15988  (((1.5707963267949) *
15989  (x752.value))) +
15990  (x753.value));
15991  sj0array[0] =
15992  IKsin(j0array[0]);
15993  cj0array[0] =
15994  IKcos(j0array[0]);
15995  if (j0array[0] > IKPI)
15996  {
15997  j0array[0] -= IK2PI;
15998  }
15999  else if (j0array[0] <
16000  -IKPI)
16001  {
16002  j0array[0] += IK2PI;
16003  }
16004  j0valid[0] = true;
16005  for (int ij0 = 0; ij0 < 1;
16006  ++ij0)
16007  {
16008  if (!j0valid[ij0])
16009  {
16010  continue;
16011  }
16012  _ij0[0] = ij0;
16013  _ij0[1] = -1;
16014  for (int iij0 = ij0 + 1;
16015  iij0 < 1;
16016  ++iij0)
16017  {
16018  if (j0valid[iij0] &&
16019  IKabs(
16020  cj0array
16021  [ij0] -
16022  cj0array
16023  [iij0]) <
16024  IKFAST_SOLUTION_THRESH &&
16025  IKabs(
16026  sj0array
16027  [ij0] -
16028  sj0array
16029  [iij0]) <
16030  IKFAST_SOLUTION_THRESH)
16031  {
16032  j0valid[iij0] =
16033  false;
16034  _ij0[1] = iij0;
16035  break;
16036  }
16037  }
16038  j0 = j0array[ij0];
16039  cj0 = cj0array[ij0];
16040  sj0 = sj0array[ij0];
16041  {
16042  IkReal evalcond[8];
16043  IkReal x754 =
16044  IKsin(j0);
16045  IkReal x755 =
16046  IKcos(j0);
16047  IkReal x756 =
16048  (gconst11 * x754);
16049  IkReal x757 =
16050  (gconst11 * x755);
16051  evalcond[0] =
16052  (new_r11 * x754);
16053  evalcond[1] =
16054  ((-1.0) * x756);
16055  evalcond[2] =
16056  (gconst11 +
16057  ((new_r11 *
16058  x755)));
16059  evalcond[3] =
16060  (x757 + new_r11);
16061  evalcond[4] =
16062  ((((-1.0) *
16063  x756)) +
16064  new_r10);
16065  evalcond[5] =
16066  ((((-1.0) *
16067  x757)) +
16068  new_r00);
16069  evalcond[6] =
16070  ((((-1.0) *
16071  new_r00 *
16072  x754)) +
16073  ((new_r10 *
16074  x755)));
16075  evalcond[7] =
16076  (((new_r00 *
16077  x755)) +
16078  ((new_r10 *
16079  x754)) +
16080  (((-1.0) *
16081  gconst11)));
16082  if (IKabs(
16083  evalcond[0]) >
16084  IKFAST_EVALCOND_THRESH ||
16085  IKabs(
16086  evalcond[1]) >
16087  IKFAST_EVALCOND_THRESH ||
16088  IKabs(
16089  evalcond[2]) >
16090  IKFAST_EVALCOND_THRESH ||
16091  IKabs(
16092  evalcond[3]) >
16093  IKFAST_EVALCOND_THRESH ||
16094  IKabs(
16095  evalcond[4]) >
16096  IKFAST_EVALCOND_THRESH ||
16097  IKabs(
16098  evalcond[5]) >
16099  IKFAST_EVALCOND_THRESH ||
16100  IKabs(
16101  evalcond[6]) >
16102  IKFAST_EVALCOND_THRESH ||
16103  IKabs(
16104  evalcond[7]) >
16105  IKFAST_EVALCOND_THRESH)
16106  {
16107  continue;
16108  }
16109  }
16110 
16111  {
16112  std::vector<
16113  IkSingleDOFSolutionBase<
16114  IkReal> >
16115  vinfos(7);
16116  vinfos[0].jointtype =
16117  1;
16118  vinfos[0].foffset =
16119  j0;
16120  vinfos[0].indices[0] =
16121  _ij0[0];
16122  vinfos[0].indices[1] =
16123  _ij0[1];
16124  vinfos[0]
16125  .maxsolutions =
16126  _nj0;
16127  vinfos[1].jointtype =
16128  1;
16129  vinfos[1].foffset =
16130  j1;
16131  vinfos[1].indices[0] =
16132  _ij1[0];
16133  vinfos[1].indices[1] =
16134  _ij1[1];
16135  vinfos[1]
16136  .maxsolutions =
16137  _nj1;
16138  vinfos[2].jointtype =
16139  1;
16140  vinfos[2].foffset =
16141  j2;
16142  vinfos[2].indices[0] =
16143  _ij2[0];
16144  vinfos[2].indices[1] =
16145  _ij2[1];
16146  vinfos[2]
16147  .maxsolutions =
16148  _nj2;
16149  vinfos[3].jointtype =
16150  1;
16151  vinfos[3].foffset =
16152  j3;
16153  vinfos[3].indices[0] =
16154  _ij3[0];
16155  vinfos[3].indices[1] =
16156  _ij3[1];
16157  vinfos[3]
16158  .maxsolutions =
16159  _nj3;
16160  vinfos[4].jointtype =
16161  1;
16162  vinfos[4].foffset =
16163  j4;
16164  vinfos[4].indices[0] =
16165  _ij4[0];
16166  vinfos[4].indices[1] =
16167  _ij4[1];
16168  vinfos[4]
16169  .maxsolutions =
16170  _nj4;
16171  vinfos[5].jointtype =
16172  1;
16173  vinfos[5].foffset =
16174  j5;
16175  vinfos[5].indices[0] =
16176  _ij5[0];
16177  vinfos[5].indices[1] =
16178  _ij5[1];
16179  vinfos[5]
16180  .maxsolutions =
16181  _nj5;
16182  vinfos[6].jointtype =
16183  1;
16184  vinfos[6].foffset =
16185  j6;
16186  vinfos[6].indices[0] =
16187  _ij6[0];
16188  vinfos[6].indices[1] =
16189  _ij6[1];
16190  vinfos[6]
16191  .maxsolutions =
16192  _nj6;
16193  std::vector<int>
16194  vfree(0);
16195  solutions.AddSolution(
16196  vinfos, vfree);
16197  }
16198  }
16199  }
16200  }
16201  }
16202  }
16203  else
16204  {
16205  {
16206  IkReal j0array[1],
16207  cj0array[1], sj0array[1];
16208  bool j0valid[1] = { false };
16209  _nj0 = 1;
16210  CheckValue<IkReal> x758 =
16211  IKPowWithIntegerCheck(
16212  IKsign(gconst11), -1);
16213  if (!x758.valid)
16214  {
16215  continue;
16216  }
16217  CheckValue<IkReal> x759 =
16218  IKatan2WithCheck(
16219  IkReal(new_r10),
16220  IkReal(new_r00),
16221  IKFAST_ATAN2_MAGTHRESH);
16222  if (!x759.valid)
16223  {
16224  continue;
16225  }
16226  j0array[0] =
16227  ((-1.5707963267949) +
16228  (((1.5707963267949) *
16229  (x758.value))) +
16230  (x759.value));
16231  sj0array[0] =
16232  IKsin(j0array[0]);
16233  cj0array[0] =
16234  IKcos(j0array[0]);
16235  if (j0array[0] > IKPI)
16236  {
16237  j0array[0] -= IK2PI;
16238  }
16239  else if (j0array[0] < -IKPI)
16240  {
16241  j0array[0] += IK2PI;
16242  }
16243  j0valid[0] = true;
16244  for (int ij0 = 0; ij0 < 1;
16245  ++ij0)
16246  {
16247  if (!j0valid[ij0])
16248  {
16249  continue;
16250  }
16251  _ij0[0] = ij0;
16252  _ij0[1] = -1;
16253  for (int iij0 = ij0 + 1;
16254  iij0 < 1;
16255  ++iij0)
16256  {
16257  if (j0valid[iij0] &&
16258  IKabs(
16259  cj0array[ij0] -
16260  cj0array[iij0]) <
16261  IKFAST_SOLUTION_THRESH &&
16262  IKabs(
16263  sj0array[ij0] -
16264  sj0array[iij0]) <
16265  IKFAST_SOLUTION_THRESH)
16266  {
16267  j0valid[iij0] = false;
16268  _ij0[1] = iij0;
16269  break;
16270  }
16271  }
16272  j0 = j0array[ij0];
16273  cj0 = cj0array[ij0];
16274  sj0 = sj0array[ij0];
16275  {
16276  IkReal evalcond[8];
16277  IkReal x760 = IKsin(j0);
16278  IkReal x761 = IKcos(j0);
16279  IkReal x762 =
16280  (gconst11 * x760);
16281  IkReal x763 =
16282  (gconst11 * x761);
16283  evalcond[0] =
16284  (new_r11 * x760);
16285  evalcond[1] =
16286  ((-1.0) * x762);
16287  evalcond[2] =
16288  (((new_r11 * x761)) +
16289  gconst11);
16290  evalcond[3] =
16291  (x763 + new_r11);
16292  evalcond[4] =
16293  ((((-1.0) * x762)) +
16294  new_r10);
16295  evalcond[5] =
16296  ((((-1.0) * x763)) +
16297  new_r00);
16298  evalcond[6] =
16299  (((new_r10 * x761)) +
16300  (((-1.0) * new_r00 *
16301  x760)));
16302  evalcond[7] =
16303  (((new_r10 * x760)) +
16304  ((new_r00 * x761)) +
16305  (((-1.0) *
16306  gconst11)));
16307  if (IKabs(evalcond[0]) >
16308  IKFAST_EVALCOND_THRESH ||
16309  IKabs(evalcond[1]) >
16310  IKFAST_EVALCOND_THRESH ||
16311  IKabs(evalcond[2]) >
16312  IKFAST_EVALCOND_THRESH ||
16313  IKabs(evalcond[3]) >
16314  IKFAST_EVALCOND_THRESH ||
16315  IKabs(evalcond[4]) >
16316  IKFAST_EVALCOND_THRESH ||
16317  IKabs(evalcond[5]) >
16318  IKFAST_EVALCOND_THRESH ||
16319  IKabs(evalcond[6]) >
16320  IKFAST_EVALCOND_THRESH ||
16321  IKabs(evalcond[7]) >
16322  IKFAST_EVALCOND_THRESH)
16323  {
16324  continue;
16325  }
16326  }
16327 
16328  {
16329  std::vector<
16330  IkSingleDOFSolutionBase<
16331  IkReal> >
16332  vinfos(7);
16333  vinfos[0].jointtype = 1;
16334  vinfos[0].foffset = j0;
16335  vinfos[0].indices[0] =
16336  _ij0[0];
16337  vinfos[0].indices[1] =
16338  _ij0[1];
16339  vinfos[0].maxsolutions =
16340  _nj0;
16341  vinfos[1].jointtype = 1;
16342  vinfos[1].foffset = j1;
16343  vinfos[1].indices[0] =
16344  _ij1[0];
16345  vinfos[1].indices[1] =
16346  _ij1[1];
16347  vinfos[1].maxsolutions =
16348  _nj1;
16349  vinfos[2].jointtype = 1;
16350  vinfos[2].foffset = j2;
16351  vinfos[2].indices[0] =
16352  _ij2[0];
16353  vinfos[2].indices[1] =
16354  _ij2[1];
16355  vinfos[2].maxsolutions =
16356  _nj2;
16357  vinfos[3].jointtype = 1;
16358  vinfos[3].foffset = j3;
16359  vinfos[3].indices[0] =
16360  _ij3[0];
16361  vinfos[3].indices[1] =
16362  _ij3[1];
16363  vinfos[3].maxsolutions =
16364  _nj3;
16365  vinfos[4].jointtype = 1;
16366  vinfos[4].foffset = j4;
16367  vinfos[4].indices[0] =
16368  _ij4[0];
16369  vinfos[4].indices[1] =
16370  _ij4[1];
16371  vinfos[4].maxsolutions =
16372  _nj4;
16373  vinfos[5].jointtype = 1;
16374  vinfos[5].foffset = j5;
16375  vinfos[5].indices[0] =
16376  _ij5[0];
16377  vinfos[5].indices[1] =
16378  _ij5[1];
16379  vinfos[5].maxsolutions =
16380  _nj5;
16381  vinfos[6].jointtype = 1;
16382  vinfos[6].foffset = j6;
16383  vinfos[6].indices[0] =
16384  _ij6[0];
16385  vinfos[6].indices[1] =
16386  _ij6[1];
16387  vinfos[6].maxsolutions =
16388  _nj6;
16389  std::vector<int> vfree(0);
16390  solutions.AddSolution(
16391  vinfos, vfree);
16392  }
16393  }
16394  }
16395  }
16396  }
16397  }
16398  } while (0);
16399  if (bgotonextstatement)
16400  {
16401  bool bgotonextstatement = true;
16402  do
16403  {
16404  if (1)
16405  {
16406  bgotonextstatement = false;
16407  continue; // branch miss [j0]
16408  }
16409  } while (0);
16410  if (bgotonextstatement)
16411  {
16412  }
16413  }
16414  }
16415  }
16416  }
16417  }
16418  }
16419  }
16420  else
16421  {
16422  {
16423  IkReal j0array[1], cj0array[1], sj0array[1];
16424  bool j0valid[1] = { false };
16425  _nj0 = 1;
16426  IkReal x764 = ((1.0) * new_r11);
16427  CheckValue<IkReal> x765 = IKPowWithIntegerCheck(
16428  IKsign(((((-1.0) * gconst11 * x764)) +
16429  ((gconst10 * new_r01)))),
16430  -1);
16431  if (!x765.valid)
16432  {
16433  continue;
16434  }
16435  CheckValue<IkReal> x766 = IKatan2WithCheck(
16436  IkReal((((gconst10 * gconst11)) +
16437  (((-1.0) * new_r01 * x764)))),
16438  IkReal(
16439  ((new_r11 * new_r11) +
16440  (((-1.0) * (gconst10 * gconst10))))),
16441  IKFAST_ATAN2_MAGTHRESH);
16442  if (!x766.valid)
16443  {
16444  continue;
16445  }
16446  j0array[0] =
16447  ((-1.5707963267949) +
16448  (((1.5707963267949) * (x765.value))) +
16449  (x766.value));
16450  sj0array[0] = IKsin(j0array[0]);
16451  cj0array[0] = IKcos(j0array[0]);
16452  if (j0array[0] > IKPI)
16453  {
16454  j0array[0] -= IK2PI;
16455  }
16456  else if (j0array[0] < -IKPI)
16457  {
16458  j0array[0] += IK2PI;
16459  }
16460  j0valid[0] = true;
16461  for (int ij0 = 0; ij0 < 1; ++ij0)
16462  {
16463  if (!j0valid[ij0])
16464  {
16465  continue;
16466  }
16467  _ij0[0] = ij0;
16468  _ij0[1] = -1;
16469  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16470  {
16471  if (j0valid[iij0] &&
16472  IKabs(cj0array[ij0] - cj0array[iij0]) <
16473  IKFAST_SOLUTION_THRESH &&
16474  IKabs(sj0array[ij0] - sj0array[iij0]) <
16475  IKFAST_SOLUTION_THRESH)
16476  {
16477  j0valid[iij0] = false;
16478  _ij0[1] = iij0;
16479  break;
16480  }
16481  }
16482  j0 = j0array[ij0];
16483  cj0 = cj0array[ij0];
16484  sj0 = sj0array[ij0];
16485  {
16486  IkReal evalcond[8];
16487  IkReal x767 = IKcos(j0);
16488  IkReal x768 = IKsin(j0);
16489  IkReal x769 = (gconst10 * x767);
16490  IkReal x770 = (gconst11 * x767);
16491  IkReal x771 = ((1.0) * x768);
16492  IkReal x772 = (gconst11 * x771);
16493  evalcond[0] =
16494  (((new_r11 * x768)) + gconst10 +
16495  ((new_r01 * x767)));
16496  evalcond[1] =
16497  (((gconst10 * x768)) + x770 + new_r11);
16498  evalcond[2] = (((new_r10 * x767)) +
16499  (((-1.0) * new_r00 * x771)) +
16500  gconst10);
16501  evalcond[3] = (((new_r11 * x767)) +
16502  (((-1.0) * new_r01 * x771)) +
16503  gconst11);
16504  evalcond[4] =
16505  (x769 + (((-1.0) * x772)) + new_r10);
16506  evalcond[5] =
16507  (x769 + (((-1.0) * x772)) + new_r01);
16508  evalcond[6] = (((new_r10 * x768)) +
16509  ((new_r00 * x767)) +
16510  (((-1.0) * gconst11)));
16511  evalcond[7] =
16512  ((((-1.0) * gconst10 * x771)) +
16513  new_r00 + (((-1.0) * x770)));
16514  if (IKabs(evalcond[0]) >
16515  IKFAST_EVALCOND_THRESH ||
16516  IKabs(evalcond[1]) >
16517  IKFAST_EVALCOND_THRESH ||
16518  IKabs(evalcond[2]) >
16519  IKFAST_EVALCOND_THRESH ||
16520  IKabs(evalcond[3]) >
16521  IKFAST_EVALCOND_THRESH ||
16522  IKabs(evalcond[4]) >
16523  IKFAST_EVALCOND_THRESH ||
16524  IKabs(evalcond[5]) >
16525  IKFAST_EVALCOND_THRESH ||
16526  IKabs(evalcond[6]) >
16527  IKFAST_EVALCOND_THRESH ||
16528  IKabs(evalcond[7]) >
16529  IKFAST_EVALCOND_THRESH)
16530  {
16531  continue;
16532  }
16533  }
16534 
16535  {
16536  std::vector<
16537  IkSingleDOFSolutionBase<IkReal> >
16538  vinfos(7);
16539  vinfos[0].jointtype = 1;
16540  vinfos[0].foffset = j0;
16541  vinfos[0].indices[0] = _ij0[0];
16542  vinfos[0].indices[1] = _ij0[1];
16543  vinfos[0].maxsolutions = _nj0;
16544  vinfos[1].jointtype = 1;
16545  vinfos[1].foffset = j1;
16546  vinfos[1].indices[0] = _ij1[0];
16547  vinfos[1].indices[1] = _ij1[1];
16548  vinfos[1].maxsolutions = _nj1;
16549  vinfos[2].jointtype = 1;
16550  vinfos[2].foffset = j2;
16551  vinfos[2].indices[0] = _ij2[0];
16552  vinfos[2].indices[1] = _ij2[1];
16553  vinfos[2].maxsolutions = _nj2;
16554  vinfos[3].jointtype = 1;
16555  vinfos[3].foffset = j3;
16556  vinfos[3].indices[0] = _ij3[0];
16557  vinfos[3].indices[1] = _ij3[1];
16558  vinfos[3].maxsolutions = _nj3;
16559  vinfos[4].jointtype = 1;
16560  vinfos[4].foffset = j4;
16561  vinfos[4].indices[0] = _ij4[0];
16562  vinfos[4].indices[1] = _ij4[1];
16563  vinfos[4].maxsolutions = _nj4;
16564  vinfos[5].jointtype = 1;
16565  vinfos[5].foffset = j5;
16566  vinfos[5].indices[0] = _ij5[0];
16567  vinfos[5].indices[1] = _ij5[1];
16568  vinfos[5].maxsolutions = _nj5;
16569  vinfos[6].jointtype = 1;
16570  vinfos[6].foffset = j6;
16571  vinfos[6].indices[0] = _ij6[0];
16572  vinfos[6].indices[1] = _ij6[1];
16573  vinfos[6].maxsolutions = _nj6;
16574  std::vector<int> vfree(0);
16575  solutions.AddSolution(vinfos, vfree);
16576  }
16577  }
16578  }
16579  }
16580  }
16581  }
16582  else
16583  {
16584  {
16585  IkReal j0array[1], cj0array[1], sj0array[1];
16586  bool j0valid[1] = { false };
16587  _nj0 = 1;
16588  IkReal x773 = ((1.0) * new_r11);
16589  CheckValue<IkReal> x774 = IKPowWithIntegerCheck(
16590  IKsign(((new_r01 * new_r01) +
16591  (new_r11 * new_r11))),
16592  -1);
16593  if (!x774.valid)
16594  {
16595  continue;
16596  }
16597  CheckValue<IkReal> x775 = IKatan2WithCheck(
16598  IkReal((((gconst11 * new_r01)) +
16599  (((-1.0) * gconst10 * x773)))),
16600  IkReal(((((-1.0) * gconst10 * new_r01)) +
16601  (((-1.0) * gconst11 * x773)))),
16602  IKFAST_ATAN2_MAGTHRESH);
16603  if (!x775.valid)
16604  {
16605  continue;
16606  }
16607  j0array[0] = ((-1.5707963267949) +
16608  (((1.5707963267949) * (x774.value))) +
16609  (x775.value));
16610  sj0array[0] = IKsin(j0array[0]);
16611  cj0array[0] = IKcos(j0array[0]);
16612  if (j0array[0] > IKPI)
16613  {
16614  j0array[0] -= IK2PI;
16615  }
16616  else if (j0array[0] < -IKPI)
16617  {
16618  j0array[0] += IK2PI;
16619  }
16620  j0valid[0] = true;
16621  for (int ij0 = 0; ij0 < 1; ++ij0)
16622  {
16623  if (!j0valid[ij0])
16624  {
16625  continue;
16626  }
16627  _ij0[0] = ij0;
16628  _ij0[1] = -1;
16629  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16630  {
16631  if (j0valid[iij0] &&
16632  IKabs(cj0array[ij0] - cj0array[iij0]) <
16633  IKFAST_SOLUTION_THRESH &&
16634  IKabs(sj0array[ij0] - sj0array[iij0]) <
16635  IKFAST_SOLUTION_THRESH)
16636  {
16637  j0valid[iij0] = false;
16638  _ij0[1] = iij0;
16639  break;
16640  }
16641  }
16642  j0 = j0array[ij0];
16643  cj0 = cj0array[ij0];
16644  sj0 = sj0array[ij0];
16645  {
16646  IkReal evalcond[8];
16647  IkReal x776 = IKcos(j0);
16648  IkReal x777 = IKsin(j0);
16649  IkReal x778 = (gconst10 * x776);
16650  IkReal x779 = (gconst11 * x776);
16651  IkReal x780 = ((1.0) * x777);
16652  IkReal x781 = (gconst11 * x780);
16653  evalcond[0] = (gconst10 + ((new_r11 * x777)) +
16654  ((new_r01 * x776)));
16655  evalcond[1] =
16656  (x779 + new_r11 + ((gconst10 * x777)));
16657  evalcond[2] = (gconst10 + ((new_r10 * x776)) +
16658  (((-1.0) * new_r00 * x780)));
16659  evalcond[3] = ((((-1.0) * new_r01 * x780)) +
16660  gconst11 + ((new_r11 * x776)));
16661  evalcond[4] =
16662  ((((-1.0) * x781)) + x778 + new_r10);
16663  evalcond[5] =
16664  ((((-1.0) * x781)) + x778 + new_r01);
16665  evalcond[6] =
16666  (((new_r00 * x776)) + ((new_r10 * x777)) +
16667  (((-1.0) * gconst11)));
16668  evalcond[7] = (new_r00 + (((-1.0) * x779)) +
16669  (((-1.0) * gconst10 * x780)));
16670  if (IKabs(evalcond[0]) >
16671  IKFAST_EVALCOND_THRESH ||
16672  IKabs(evalcond[1]) >
16673  IKFAST_EVALCOND_THRESH ||
16674  IKabs(evalcond[2]) >
16675  IKFAST_EVALCOND_THRESH ||
16676  IKabs(evalcond[3]) >
16677  IKFAST_EVALCOND_THRESH ||
16678  IKabs(evalcond[4]) >
16679  IKFAST_EVALCOND_THRESH ||
16680  IKabs(evalcond[5]) >
16681  IKFAST_EVALCOND_THRESH ||
16682  IKabs(evalcond[6]) >
16683  IKFAST_EVALCOND_THRESH ||
16684  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16685  {
16686  continue;
16687  }
16688  }
16689 
16690  {
16691  std::vector<IkSingleDOFSolutionBase<IkReal> >
16692  vinfos(7);
16693  vinfos[0].jointtype = 1;
16694  vinfos[0].foffset = j0;
16695  vinfos[0].indices[0] = _ij0[0];
16696  vinfos[0].indices[1] = _ij0[1];
16697  vinfos[0].maxsolutions = _nj0;
16698  vinfos[1].jointtype = 1;
16699  vinfos[1].foffset = j1;
16700  vinfos[1].indices[0] = _ij1[0];
16701  vinfos[1].indices[1] = _ij1[1];
16702  vinfos[1].maxsolutions = _nj1;
16703  vinfos[2].jointtype = 1;
16704  vinfos[2].foffset = j2;
16705  vinfos[2].indices[0] = _ij2[0];
16706  vinfos[2].indices[1] = _ij2[1];
16707  vinfos[2].maxsolutions = _nj2;
16708  vinfos[3].jointtype = 1;
16709  vinfos[3].foffset = j3;
16710  vinfos[3].indices[0] = _ij3[0];
16711  vinfos[3].indices[1] = _ij3[1];
16712  vinfos[3].maxsolutions = _nj3;
16713  vinfos[4].jointtype = 1;
16714  vinfos[4].foffset = j4;
16715  vinfos[4].indices[0] = _ij4[0];
16716  vinfos[4].indices[1] = _ij4[1];
16717  vinfos[4].maxsolutions = _nj4;
16718  vinfos[5].jointtype = 1;
16719  vinfos[5].foffset = j5;
16720  vinfos[5].indices[0] = _ij5[0];
16721  vinfos[5].indices[1] = _ij5[1];
16722  vinfos[5].maxsolutions = _nj5;
16723  vinfos[6].jointtype = 1;
16724  vinfos[6].foffset = j6;
16725  vinfos[6].indices[0] = _ij6[0];
16726  vinfos[6].indices[1] = _ij6[1];
16727  vinfos[6].maxsolutions = _nj6;
16728  std::vector<int> vfree(0);
16729  solutions.AddSolution(vinfos, vfree);
16730  }
16731  }
16732  }
16733  }
16734  }
16735  }
16736  else
16737  {
16738  {
16739  IkReal j0array[1], cj0array[1], sj0array[1];
16740  bool j0valid[1] = { false };
16741  _nj0 = 1;
16742  IkReal x782 = ((1.0) * gconst10);
16743  CheckValue<IkReal> x783 = IKatan2WithCheck(
16744  IkReal(((((-1.0) * new_r10 * x782)) +
16745  ((gconst10 * new_r01)))),
16746  IkReal(((((-1.0) * new_r11 * x782)) +
16747  (((-1.0) * new_r00 * x782)))),
16748  IKFAST_ATAN2_MAGTHRESH);
16749  if (!x783.valid)
16750  {
16751  continue;
16752  }
16753  CheckValue<IkReal> x784 = IKPowWithIntegerCheck(
16754  IKsign((((new_r10 * new_r11)) +
16755  ((new_r00 * new_r01)))),
16756  -1);
16757  if (!x784.valid)
16758  {
16759  continue;
16760  }
16761  j0array[0] = ((-1.5707963267949) + (x783.value) +
16762  (((1.5707963267949) * (x784.value))));
16763  sj0array[0] = IKsin(j0array[0]);
16764  cj0array[0] = IKcos(j0array[0]);
16765  if (j0array[0] > IKPI)
16766  {
16767  j0array[0] -= IK2PI;
16768  }
16769  else if (j0array[0] < -IKPI)
16770  {
16771  j0array[0] += IK2PI;
16772  }
16773  j0valid[0] = true;
16774  for (int ij0 = 0; ij0 < 1; ++ij0)
16775  {
16776  if (!j0valid[ij0])
16777  {
16778  continue;
16779  }
16780  _ij0[0] = ij0;
16781  _ij0[1] = -1;
16782  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16783  {
16784  if (j0valid[iij0] &&
16785  IKabs(cj0array[ij0] - cj0array[iij0]) <
16786  IKFAST_SOLUTION_THRESH &&
16787  IKabs(sj0array[ij0] - sj0array[iij0]) <
16788  IKFAST_SOLUTION_THRESH)
16789  {
16790  j0valid[iij0] = false;
16791  _ij0[1] = iij0;
16792  break;
16793  }
16794  }
16795  j0 = j0array[ij0];
16796  cj0 = cj0array[ij0];
16797  sj0 = sj0array[ij0];
16798  {
16799  IkReal evalcond[8];
16800  IkReal x785 = IKcos(j0);
16801  IkReal x786 = IKsin(j0);
16802  IkReal x787 = (gconst10 * x785);
16803  IkReal x788 = (gconst11 * x785);
16804  IkReal x789 = ((1.0) * x786);
16805  IkReal x790 = (gconst11 * x789);
16806  evalcond[0] = (((new_r11 * x786)) + gconst10 +
16807  ((new_r01 * x785)));
16808  evalcond[1] =
16809  (((gconst10 * x786)) + x788 + new_r11);
16810  evalcond[2] = (((new_r10 * x785)) + gconst10 +
16811  (((-1.0) * new_r00 * x789)));
16812  evalcond[3] = ((((-1.0) * new_r01 * x789)) +
16813  ((new_r11 * x785)) + gconst11);
16814  evalcond[4] = ((((-1.0) * x790)) + x787 + new_r10);
16815  evalcond[5] = ((((-1.0) * x790)) + x787 + new_r01);
16816  evalcond[6] =
16817  (((new_r10 * x786)) + ((new_r00 * x785)) +
16818  (((-1.0) * gconst11)));
16819  evalcond[7] = ((((-1.0) * x788)) + new_r00 +
16820  (((-1.0) * gconst10 * x789)));
16821  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
16822  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
16823  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
16824  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
16825  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
16826  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
16827  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
16828  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16829  {
16830  continue;
16831  }
16832  }
16833 
16834  {
16835  std::vector<IkSingleDOFSolutionBase<IkReal> >
16836  vinfos(7);
16837  vinfos[0].jointtype = 1;
16838  vinfos[0].foffset = j0;
16839  vinfos[0].indices[0] = _ij0[0];
16840  vinfos[0].indices[1] = _ij0[1];
16841  vinfos[0].maxsolutions = _nj0;
16842  vinfos[1].jointtype = 1;
16843  vinfos[1].foffset = j1;
16844  vinfos[1].indices[0] = _ij1[0];
16845  vinfos[1].indices[1] = _ij1[1];
16846  vinfos[1].maxsolutions = _nj1;
16847  vinfos[2].jointtype = 1;
16848  vinfos[2].foffset = j2;
16849  vinfos[2].indices[0] = _ij2[0];
16850  vinfos[2].indices[1] = _ij2[1];
16851  vinfos[2].maxsolutions = _nj2;
16852  vinfos[3].jointtype = 1;
16853  vinfos[3].foffset = j3;
16854  vinfos[3].indices[0] = _ij3[0];
16855  vinfos[3].indices[1] = _ij3[1];
16856  vinfos[3].maxsolutions = _nj3;
16857  vinfos[4].jointtype = 1;
16858  vinfos[4].foffset = j4;
16859  vinfos[4].indices[0] = _ij4[0];
16860  vinfos[4].indices[1] = _ij4[1];
16861  vinfos[4].maxsolutions = _nj4;
16862  vinfos[5].jointtype = 1;
16863  vinfos[5].foffset = j5;
16864  vinfos[5].indices[0] = _ij5[0];
16865  vinfos[5].indices[1] = _ij5[1];
16866  vinfos[5].maxsolutions = _nj5;
16867  vinfos[6].jointtype = 1;
16868  vinfos[6].foffset = j6;
16869  vinfos[6].indices[0] = _ij6[0];
16870  vinfos[6].indices[1] = _ij6[1];
16871  vinfos[6].maxsolutions = _nj6;
16872  std::vector<int> vfree(0);
16873  solutions.AddSolution(vinfos, vfree);
16874  }
16875  }
16876  }
16877  }
16878  }
16879  }
16880  } while (0);
16881  if (bgotonextstatement)
16882  {
16883  bool bgotonextstatement = true;
16884  do
16885  {
16886  evalcond[0] = ((new_r01 * new_r01) + (new_r11 * new_r11));
16887  if (IKabs(evalcond[0]) < 0.0000050000000000)
16888  {
16889  bgotonextstatement = false;
16890  {
16891  IkReal j0eval[1];
16892  sj1 = 0;
16893  cj1 = -1.0;
16894  j1 = 3.14159265358979;
16895  new_r01 = 0;
16896  new_r11 = 0;
16897  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
16898  if (IKabs(j0eval[0]) < 0.0000010000000000)
16899  {
16900  continue; // 3 cases reached
16901  }
16902  else
16903  {
16904  {
16905  IkReal j0array[2], cj0array[2], sj0array[2];
16906  bool j0valid[2] = { false };
16907  _nj0 = 2;
16908  CheckValue<IkReal> x792 =
16909  IKatan2WithCheck(IkReal(new_r00),
16910  IkReal(new_r10),
16911  IKFAST_ATAN2_MAGTHRESH);
16912  if (!x792.valid)
16913  {
16914  continue;
16915  }
16916  IkReal x791 = x792.value;
16917  j0array[0] = ((-1.0) * x791);
16918  sj0array[0] = IKsin(j0array[0]);
16919  cj0array[0] = IKcos(j0array[0]);
16920  j0array[1] = ((3.14159265358979) + (((-1.0) * x791)));
16921  sj0array[1] = IKsin(j0array[1]);
16922  cj0array[1] = IKcos(j0array[1]);
16923  if (j0array[0] > IKPI)
16924  {
16925  j0array[0] -= IK2PI;
16926  }
16927  else if (j0array[0] < -IKPI)
16928  {
16929  j0array[0] += IK2PI;
16930  }
16931  j0valid[0] = true;
16932  if (j0array[1] > IKPI)
16933  {
16934  j0array[1] -= IK2PI;
16935  }
16936  else if (j0array[1] < -IKPI)
16937  {
16938  j0array[1] += IK2PI;
16939  }
16940  j0valid[1] = true;
16941  for (int ij0 = 0; ij0 < 2; ++ij0)
16942  {
16943  if (!j0valid[ij0])
16944  {
16945  continue;
16946  }
16947  _ij0[0] = ij0;
16948  _ij0[1] = -1;
16949  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
16950  {
16951  if (j0valid[iij0] &&
16952  IKabs(cj0array[ij0] - cj0array[iij0]) <
16953  IKFAST_SOLUTION_THRESH &&
16954  IKabs(sj0array[ij0] - sj0array[iij0]) <
16955  IKFAST_SOLUTION_THRESH)
16956  {
16957  j0valid[iij0] = false;
16958  _ij0[1] = iij0;
16959  break;
16960  }
16961  }
16962  j0 = j0array[ij0];
16963  cj0 = cj0array[ij0];
16964  sj0 = sj0array[ij0];
16965  {
16966  IkReal evalcond[1];
16967  evalcond[0] =
16968  ((((-1.0) * new_r00 * (IKsin(j0)))) +
16969  ((new_r10 * (IKcos(j0)))));
16970  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
16971  {
16972  continue;
16973  }
16974  }
16975 
16976  {
16977  std::vector<IkSingleDOFSolutionBase<IkReal> >
16978  vinfos(7);
16979  vinfos[0].jointtype = 1;
16980  vinfos[0].foffset = j0;
16981  vinfos[0].indices[0] = _ij0[0];
16982  vinfos[0].indices[1] = _ij0[1];
16983  vinfos[0].maxsolutions = _nj0;
16984  vinfos[1].jointtype = 1;
16985  vinfos[1].foffset = j1;
16986  vinfos[1].indices[0] = _ij1[0];
16987  vinfos[1].indices[1] = _ij1[1];
16988  vinfos[1].maxsolutions = _nj1;
16989  vinfos[2].jointtype = 1;
16990  vinfos[2].foffset = j2;
16991  vinfos[2].indices[0] = _ij2[0];
16992  vinfos[2].indices[1] = _ij2[1];
16993  vinfos[2].maxsolutions = _nj2;
16994  vinfos[3].jointtype = 1;
16995  vinfos[3].foffset = j3;
16996  vinfos[3].indices[0] = _ij3[0];
16997  vinfos[3].indices[1] = _ij3[1];
16998  vinfos[3].maxsolutions = _nj3;
16999  vinfos[4].jointtype = 1;
17000  vinfos[4].foffset = j4;
17001  vinfos[4].indices[0] = _ij4[0];
17002  vinfos[4].indices[1] = _ij4[1];
17003  vinfos[4].maxsolutions = _nj4;
17004  vinfos[5].jointtype = 1;
17005  vinfos[5].foffset = j5;
17006  vinfos[5].indices[0] = _ij5[0];
17007  vinfos[5].indices[1] = _ij5[1];
17008  vinfos[5].maxsolutions = _nj5;
17009  vinfos[6].jointtype = 1;
17010  vinfos[6].foffset = j6;
17011  vinfos[6].indices[0] = _ij6[0];
17012  vinfos[6].indices[1] = _ij6[1];
17013  vinfos[6].maxsolutions = _nj6;
17014  std::vector<int> vfree(0);
17015  solutions.AddSolution(vinfos, vfree);
17016  }
17017  }
17018  }
17019  }
17020  }
17021  }
17022  } while (0);
17023  if (bgotonextstatement)
17024  {
17025  bool bgotonextstatement = true;
17026  do
17027  {
17028  evalcond[0] = ((-3.14159265358979) +
17029  (IKfmod(((3.14159265358979) + (IKabs(j2))),
17030  6.28318530717959)));
17031  if (IKabs(evalcond[0]) < 0.0000050000000000)
17032  {
17033  bgotonextstatement = false;
17034  {
17035  IkReal j0array[1], cj0array[1], sj0array[1];
17036  bool j0valid[1] = { false };
17037  _nj0 = 1;
17038  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
17039  IKabs(((-1.0) * new_r11)) <
17040  IKFAST_ATAN2_MAGTHRESH &&
17041  IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) -
17042  1) <= IKFAST_SINCOS_THRESH)
17043  continue;
17044  j0array[0] = IKatan2(new_r10, ((-1.0) * new_r11));
17045  sj0array[0] = IKsin(j0array[0]);
17046  cj0array[0] = IKcos(j0array[0]);
17047  if (j0array[0] > IKPI)
17048  {
17049  j0array[0] -= IK2PI;
17050  }
17051  else if (j0array[0] < -IKPI)
17052  {
17053  j0array[0] += IK2PI;
17054  }
17055  j0valid[0] = true;
17056  for (int ij0 = 0; ij0 < 1; ++ij0)
17057  {
17058  if (!j0valid[ij0])
17059  {
17060  continue;
17061  }
17062  _ij0[0] = ij0;
17063  _ij0[1] = -1;
17064  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17065  {
17066  if (j0valid[iij0] &&
17067  IKabs(cj0array[ij0] - cj0array[iij0]) <
17068  IKFAST_SOLUTION_THRESH &&
17069  IKabs(sj0array[ij0] - sj0array[iij0]) <
17070  IKFAST_SOLUTION_THRESH)
17071  {
17072  j0valid[iij0] = false;
17073  _ij0[1] = iij0;
17074  break;
17075  }
17076  }
17077  j0 = j0array[ij0];
17078  cj0 = cj0array[ij0];
17079  sj0 = sj0array[ij0];
17080  {
17081  IkReal evalcond[8];
17082  IkReal x793 = IKcos(j0);
17083  IkReal x794 = IKsin(j0);
17084  IkReal x795 = ((1.0) * x794);
17085  evalcond[0] = (x793 + new_r11);
17086  evalcond[1] = ((((-1.0) * x795)) + new_r10);
17087  evalcond[2] = ((((-1.0) * x793)) + new_r00);
17088  evalcond[3] = ((((-1.0) * x795)) + new_r01);
17089  evalcond[4] =
17090  (((new_r11 * x794)) + ((new_r01 * x793)));
17091  evalcond[5] = (((new_r10 * x793)) +
17092  (((-1.0) * new_r00 * x795)));
17093  evalcond[6] = ((-1.0) + ((new_r10 * x794)) +
17094  ((new_r00 * x793)));
17095  evalcond[7] = ((1.0) + (((-1.0) * new_r01 * x795)) +
17096  ((new_r11 * x793)));
17097  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17098  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17099  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17100  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17101  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17102  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17103  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17104  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17105  {
17106  continue;
17107  }
17108  }
17109 
17110  {
17111  std::vector<IkSingleDOFSolutionBase<IkReal> >
17112  vinfos(7);
17113  vinfos[0].jointtype = 1;
17114  vinfos[0].foffset = j0;
17115  vinfos[0].indices[0] = _ij0[0];
17116  vinfos[0].indices[1] = _ij0[1];
17117  vinfos[0].maxsolutions = _nj0;
17118  vinfos[1].jointtype = 1;
17119  vinfos[1].foffset = j1;
17120  vinfos[1].indices[0] = _ij1[0];
17121  vinfos[1].indices[1] = _ij1[1];
17122  vinfos[1].maxsolutions = _nj1;
17123  vinfos[2].jointtype = 1;
17124  vinfos[2].foffset = j2;
17125  vinfos[2].indices[0] = _ij2[0];
17126  vinfos[2].indices[1] = _ij2[1];
17127  vinfos[2].maxsolutions = _nj2;
17128  vinfos[3].jointtype = 1;
17129  vinfos[3].foffset = j3;
17130  vinfos[3].indices[0] = _ij3[0];
17131  vinfos[3].indices[1] = _ij3[1];
17132  vinfos[3].maxsolutions = _nj3;
17133  vinfos[4].jointtype = 1;
17134  vinfos[4].foffset = j4;
17135  vinfos[4].indices[0] = _ij4[0];
17136  vinfos[4].indices[1] = _ij4[1];
17137  vinfos[4].maxsolutions = _nj4;
17138  vinfos[5].jointtype = 1;
17139  vinfos[5].foffset = j5;
17140  vinfos[5].indices[0] = _ij5[0];
17141  vinfos[5].indices[1] = _ij5[1];
17142  vinfos[5].maxsolutions = _nj5;
17143  vinfos[6].jointtype = 1;
17144  vinfos[6].foffset = j6;
17145  vinfos[6].indices[0] = _ij6[0];
17146  vinfos[6].indices[1] = _ij6[1];
17147  vinfos[6].maxsolutions = _nj6;
17148  std::vector<int> vfree(0);
17149  solutions.AddSolution(vinfos, vfree);
17150  }
17151  }
17152  }
17153  }
17154  } while (0);
17155  if (bgotonextstatement)
17156  {
17157  bool bgotonextstatement = true;
17158  do
17159  {
17160  evalcond[0] =
17161  ((-3.14159265358979) +
17162  (IKfmod(((3.14159265358979) +
17163  (IKabs(((-3.14159265358979) + j2)))),
17164  6.28318530717959)));
17165  if (IKabs(evalcond[0]) < 0.0000050000000000)
17166  {
17167  bgotonextstatement = false;
17168  {
17169  IkReal j0array[1], cj0array[1], sj0array[1];
17170  bool j0valid[1] = { false };
17171  _nj0 = 1;
17172  if (IKabs(((-1.0) * new_r10)) <
17173  IKFAST_ATAN2_MAGTHRESH &&
17174  IKabs(((-1.0) * new_r00)) <
17175  IKFAST_ATAN2_MAGTHRESH &&
17176  IKabs(IKsqr(((-1.0) * new_r10)) +
17177  IKsqr(((-1.0) * new_r00)) - 1) <=
17178  IKFAST_SINCOS_THRESH)
17179  continue;
17180  j0array[0] =
17181  IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
17182  sj0array[0] = IKsin(j0array[0]);
17183  cj0array[0] = IKcos(j0array[0]);
17184  if (j0array[0] > IKPI)
17185  {
17186  j0array[0] -= IK2PI;
17187  }
17188  else if (j0array[0] < -IKPI)
17189  {
17190  j0array[0] += IK2PI;
17191  }
17192  j0valid[0] = true;
17193  for (int ij0 = 0; ij0 < 1; ++ij0)
17194  {
17195  if (!j0valid[ij0])
17196  {
17197  continue;
17198  }
17199  _ij0[0] = ij0;
17200  _ij0[1] = -1;
17201  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17202  {
17203  if (j0valid[iij0] &&
17204  IKabs(cj0array[ij0] - cj0array[iij0]) <
17205  IKFAST_SOLUTION_THRESH &&
17206  IKabs(sj0array[ij0] - sj0array[iij0]) <
17207  IKFAST_SOLUTION_THRESH)
17208  {
17209  j0valid[iij0] = false;
17210  _ij0[1] = iij0;
17211  break;
17212  }
17213  }
17214  j0 = j0array[ij0];
17215  cj0 = cj0array[ij0];
17216  sj0 = sj0array[ij0];
17217  {
17218  IkReal evalcond[8];
17219  IkReal x796 = IKsin(j0);
17220  IkReal x797 = IKcos(j0);
17221  IkReal x798 = ((1.0) * x796);
17222  evalcond[0] = (x796 + new_r10);
17223  evalcond[1] = (x797 + new_r00);
17224  evalcond[2] = (x796 + new_r01);
17225  evalcond[3] = ((((-1.0) * x797)) + new_r11);
17226  evalcond[4] =
17227  (((new_r11 * x796)) + ((new_r01 * x797)));
17228  evalcond[5] = (((new_r10 * x797)) +
17229  (((-1.0) * new_r00 * x798)));
17230  evalcond[6] = ((1.0) + ((new_r10 * x796)) +
17231  ((new_r00 * x797)));
17232  evalcond[7] =
17233  ((-1.0) + (((-1.0) * new_r01 * x798)) +
17234  ((new_r11 * x797)));
17235  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17236  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17237  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17238  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17239  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17240  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17241  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17242  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17243  {
17244  continue;
17245  }
17246  }
17247 
17248  {
17249  std::vector<IkSingleDOFSolutionBase<IkReal> >
17250  vinfos(7);
17251  vinfos[0].jointtype = 1;
17252  vinfos[0].foffset = j0;
17253  vinfos[0].indices[0] = _ij0[0];
17254  vinfos[0].indices[1] = _ij0[1];
17255  vinfos[0].maxsolutions = _nj0;
17256  vinfos[1].jointtype = 1;
17257  vinfos[1].foffset = j1;
17258  vinfos[1].indices[0] = _ij1[0];
17259  vinfos[1].indices[1] = _ij1[1];
17260  vinfos[1].maxsolutions = _nj1;
17261  vinfos[2].jointtype = 1;
17262  vinfos[2].foffset = j2;
17263  vinfos[2].indices[0] = _ij2[0];
17264  vinfos[2].indices[1] = _ij2[1];
17265  vinfos[2].maxsolutions = _nj2;
17266  vinfos[3].jointtype = 1;
17267  vinfos[3].foffset = j3;
17268  vinfos[3].indices[0] = _ij3[0];
17269  vinfos[3].indices[1] = _ij3[1];
17270  vinfos[3].maxsolutions = _nj3;
17271  vinfos[4].jointtype = 1;
17272  vinfos[4].foffset = j4;
17273  vinfos[4].indices[0] = _ij4[0];
17274  vinfos[4].indices[1] = _ij4[1];
17275  vinfos[4].maxsolutions = _nj4;
17276  vinfos[5].jointtype = 1;
17277  vinfos[5].foffset = j5;
17278  vinfos[5].indices[0] = _ij5[0];
17279  vinfos[5].indices[1] = _ij5[1];
17280  vinfos[5].maxsolutions = _nj5;
17281  vinfos[6].jointtype = 1;
17282  vinfos[6].foffset = j6;
17283  vinfos[6].indices[0] = _ij6[0];
17284  vinfos[6].indices[1] = _ij6[1];
17285  vinfos[6].maxsolutions = _nj6;
17286  std::vector<int> vfree(0);
17287  solutions.AddSolution(vinfos, vfree);
17288  }
17289  }
17290  }
17291  }
17292  } while (0);
17293  if (bgotonextstatement)
17294  {
17295  bool bgotonextstatement = true;
17296  do
17297  {
17298  evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r00)));
17299  if (IKabs(evalcond[0]) < 0.0000050000000000)
17300  {
17301  bgotonextstatement = false;
17302  {
17303  IkReal j0eval[3];
17304  sj1 = 0;
17305  cj1 = -1.0;
17306  j1 = 3.14159265358979;
17307  new_r11 = 0;
17308  new_r00 = 0;
17309  j0eval[0] = new_r01;
17310  j0eval[1] = IKsign(new_r01);
17311  j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
17312  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17313  IKabs(j0eval[1]) < 0.0000010000000000 ||
17314  IKabs(j0eval[2]) < 0.0000010000000000)
17315  {
17316  {
17317  IkReal j0eval[3];
17318  sj1 = 0;
17319  cj1 = -1.0;
17320  j1 = 3.14159265358979;
17321  new_r11 = 0;
17322  new_r00 = 0;
17323  j0eval[0] = new_r10;
17324  j0eval[1] = ((IKabs(cj2)) + (IKabs(sj2)));
17325  j0eval[2] = IKsign(new_r10);
17326  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17327  IKabs(j0eval[1]) < 0.0000010000000000 ||
17328  IKabs(j0eval[2]) < 0.0000010000000000)
17329  {
17330  {
17331  IkReal j0eval[2];
17332  sj1 = 0;
17333  cj1 = -1.0;
17334  j1 = 3.14159265358979;
17335  new_r11 = 0;
17336  new_r00 = 0;
17337  j0eval[0] = new_r01;
17338  j0eval[1] = new_r10;
17339  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17340  IKabs(j0eval[1]) < 0.0000010000000000)
17341  {
17342  continue; // no branches [j0]
17343  }
17344  else
17345  {
17346  {
17347  IkReal j0array[1], cj0array[1],
17348  sj0array[1];
17349  bool j0valid[1] = { false };
17350  _nj0 = 1;
17351  CheckValue<IkReal> x799 =
17352  IKPowWithIntegerCheck(new_r01, -1);
17353  if (!x799.valid)
17354  {
17355  continue;
17356  }
17357  CheckValue<IkReal> x800 =
17358  IKPowWithIntegerCheck(new_r10, -1);
17359  if (!x800.valid)
17360  {
17361  continue;
17362  }
17363  if (IKabs((cj2 * (x799.value))) <
17364  IKFAST_ATAN2_MAGTHRESH &&
17365  IKabs(
17366  ((-1.0) * sj2 * (x800.value))) <
17367  IKFAST_ATAN2_MAGTHRESH &&
17368  IKabs(IKsqr((cj2 * (x799.value))) +
17369  IKsqr(((-1.0) * sj2 *
17370  (x800.value))) -
17371  1) <= IKFAST_SINCOS_THRESH)
17372  continue;
17373  j0array[0] = IKatan2(
17374  (cj2 * (x799.value)),
17375  ((-1.0) * sj2 * (x800.value)));
17376  sj0array[0] = IKsin(j0array[0]);
17377  cj0array[0] = IKcos(j0array[0]);
17378  if (j0array[0] > IKPI)
17379  {
17380  j0array[0] -= IK2PI;
17381  }
17382  else if (j0array[0] < -IKPI)
17383  {
17384  j0array[0] += IK2PI;
17385  }
17386  j0valid[0] = true;
17387  for (int ij0 = 0; ij0 < 1; ++ij0)
17388  {
17389  if (!j0valid[ij0])
17390  {
17391  continue;
17392  }
17393  _ij0[0] = ij0;
17394  _ij0[1] = -1;
17395  for (int iij0 = ij0 + 1; iij0 < 1;
17396  ++iij0)
17397  {
17398  if (j0valid[iij0] &&
17399  IKabs(cj0array[ij0] -
17400  cj0array[iij0]) <
17401  IKFAST_SOLUTION_THRESH &&
17402  IKabs(sj0array[ij0] -
17403  sj0array[iij0]) <
17404  IKFAST_SOLUTION_THRESH)
17405  {
17406  j0valid[iij0] = false;
17407  _ij0[1] = iij0;
17408  break;
17409  }
17410  }
17411  j0 = j0array[ij0];
17412  cj0 = cj0array[ij0];
17413  sj0 = sj0array[ij0];
17414  {
17415  IkReal evalcond[7];
17416  IkReal x801 = IKcos(j0);
17417  IkReal x802 = IKsin(j0);
17418  IkReal x803 = ((1.0) * cj2);
17419  IkReal x804 = (sj2 * x801);
17420  IkReal x805 = ((1.0) * x802);
17421  IkReal x806 = (x802 * x803);
17422  evalcond[0] =
17423  (sj2 + ((new_r10 * x801)));
17424  evalcond[1] =
17425  (sj2 + ((new_r01 * x801)));
17426  evalcond[2] =
17427  ((((-1.0) * new_r01 * x805)) +
17428  cj2);
17429  evalcond[3] = (((new_r10 * x802)) +
17430  (((-1.0) * x803)));
17431  evalcond[4] = (new_r10 + x804 +
17432  (((-1.0) * x806)));
17433  evalcond[5] =
17434  ((((-1.0) * sj2 * x805)) +
17435  (((-1.0) * x801 * x803)));
17436  evalcond[6] = (new_r01 + x804 +
17437  (((-1.0) * x806)));
17438  if (IKabs(evalcond[0]) >
17439  IKFAST_EVALCOND_THRESH ||
17440  IKabs(evalcond[1]) >
17441  IKFAST_EVALCOND_THRESH ||
17442  IKabs(evalcond[2]) >
17443  IKFAST_EVALCOND_THRESH ||
17444  IKabs(evalcond[3]) >
17445  IKFAST_EVALCOND_THRESH ||
17446  IKabs(evalcond[4]) >
17447  IKFAST_EVALCOND_THRESH ||
17448  IKabs(evalcond[5]) >
17449  IKFAST_EVALCOND_THRESH ||
17450  IKabs(evalcond[6]) >
17451  IKFAST_EVALCOND_THRESH)
17452  {
17453  continue;
17454  }
17455  }
17456 
17457  {
17458  std::vector<IkSingleDOFSolutionBase<
17459  IkReal> >
17460  vinfos(7);
17461  vinfos[0].jointtype = 1;
17462  vinfos[0].foffset = j0;
17463  vinfos[0].indices[0] = _ij0[0];
17464  vinfos[0].indices[1] = _ij0[1];
17465  vinfos[0].maxsolutions = _nj0;
17466  vinfos[1].jointtype = 1;
17467  vinfos[1].foffset = j1;
17468  vinfos[1].indices[0] = _ij1[0];
17469  vinfos[1].indices[1] = _ij1[1];
17470  vinfos[1].maxsolutions = _nj1;
17471  vinfos[2].jointtype = 1;
17472  vinfos[2].foffset = j2;
17473  vinfos[2].indices[0] = _ij2[0];
17474  vinfos[2].indices[1] = _ij2[1];
17475  vinfos[2].maxsolutions = _nj2;
17476  vinfos[3].jointtype = 1;
17477  vinfos[3].foffset = j3;
17478  vinfos[3].indices[0] = _ij3[0];
17479  vinfos[3].indices[1] = _ij3[1];
17480  vinfos[3].maxsolutions = _nj3;
17481  vinfos[4].jointtype = 1;
17482  vinfos[4].foffset = j4;
17483  vinfos[4].indices[0] = _ij4[0];
17484  vinfos[4].indices[1] = _ij4[1];
17485  vinfos[4].maxsolutions = _nj4;
17486  vinfos[5].jointtype = 1;
17487  vinfos[5].foffset = j5;
17488  vinfos[5].indices[0] = _ij5[0];
17489  vinfos[5].indices[1] = _ij5[1];
17490  vinfos[5].maxsolutions = _nj5;
17491  vinfos[6].jointtype = 1;
17492  vinfos[6].foffset = j6;
17493  vinfos[6].indices[0] = _ij6[0];
17494  vinfos[6].indices[1] = _ij6[1];
17495  vinfos[6].maxsolutions = _nj6;
17496  std::vector<int> vfree(0);
17497  solutions.AddSolution(vinfos,
17498  vfree);
17499  }
17500  }
17501  }
17502  }
17503  }
17504  }
17505  else
17506  {
17507  {
17508  IkReal j0array[1], cj0array[1], sj0array[1];
17509  bool j0valid[1] = { false };
17510  _nj0 = 1;
17511  CheckValue<IkReal> x807 =
17512  IKPowWithIntegerCheck(IKsign(new_r10),
17513  -1);
17514  if (!x807.valid)
17515  {
17516  continue;
17517  }
17518  CheckValue<IkReal> x808 = IKatan2WithCheck(
17519  IkReal(cj2),
17520  IkReal(((-1.0) * sj2)),
17521  IKFAST_ATAN2_MAGTHRESH);
17522  if (!x808.valid)
17523  {
17524  continue;
17525  }
17526  j0array[0] =
17527  ((-1.5707963267949) +
17528  (((1.5707963267949) * (x807.value))) +
17529  (x808.value));
17530  sj0array[0] = IKsin(j0array[0]);
17531  cj0array[0] = IKcos(j0array[0]);
17532  if (j0array[0] > IKPI)
17533  {
17534  j0array[0] -= IK2PI;
17535  }
17536  else if (j0array[0] < -IKPI)
17537  {
17538  j0array[0] += IK2PI;
17539  }
17540  j0valid[0] = true;
17541  for (int ij0 = 0; ij0 < 1; ++ij0)
17542  {
17543  if (!j0valid[ij0])
17544  {
17545  continue;
17546  }
17547  _ij0[0] = ij0;
17548  _ij0[1] = -1;
17549  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17550  {
17551  if (j0valid[iij0] &&
17552  IKabs(cj0array[ij0] -
17553  cj0array[iij0]) <
17554  IKFAST_SOLUTION_THRESH &&
17555  IKabs(sj0array[ij0] -
17556  sj0array[iij0]) <
17557  IKFAST_SOLUTION_THRESH)
17558  {
17559  j0valid[iij0] = false;
17560  _ij0[1] = iij0;
17561  break;
17562  }
17563  }
17564  j0 = j0array[ij0];
17565  cj0 = cj0array[ij0];
17566  sj0 = sj0array[ij0];
17567  {
17568  IkReal evalcond[7];
17569  IkReal x809 = IKcos(j0);
17570  IkReal x810 = IKsin(j0);
17571  IkReal x811 = ((1.0) * cj2);
17572  IkReal x812 = (sj2 * x809);
17573  IkReal x813 = ((1.0) * x810);
17574  IkReal x814 = (x810 * x811);
17575  evalcond[0] =
17576  (sj2 + ((new_r10 * x809)));
17577  evalcond[1] =
17578  (sj2 + ((new_r01 * x809)));
17579  evalcond[2] =
17580  (cj2 + (((-1.0) * new_r01 * x813)));
17581  evalcond[3] = (((new_r10 * x810)) +
17582  (((-1.0) * x811)));
17583  evalcond[4] = ((((-1.0) * x814)) +
17584  new_r10 + x812);
17585  evalcond[5] =
17586  ((((-1.0) * x809 * x811)) +
17587  (((-1.0) * sj2 * x813)));
17588  evalcond[6] = ((((-1.0) * x814)) +
17589  new_r01 + x812);
17590  if (IKabs(evalcond[0]) >
17591  IKFAST_EVALCOND_THRESH ||
17592  IKabs(evalcond[1]) >
17593  IKFAST_EVALCOND_THRESH ||
17594  IKabs(evalcond[2]) >
17595  IKFAST_EVALCOND_THRESH ||
17596  IKabs(evalcond[3]) >
17597  IKFAST_EVALCOND_THRESH ||
17598  IKabs(evalcond[4]) >
17599  IKFAST_EVALCOND_THRESH ||
17600  IKabs(evalcond[5]) >
17601  IKFAST_EVALCOND_THRESH ||
17602  IKabs(evalcond[6]) >
17603  IKFAST_EVALCOND_THRESH)
17604  {
17605  continue;
17606  }
17607  }
17608 
17609  {
17610  std::vector<
17611  IkSingleDOFSolutionBase<IkReal> >
17612  vinfos(7);
17613  vinfos[0].jointtype = 1;
17614  vinfos[0].foffset = j0;
17615  vinfos[0].indices[0] = _ij0[0];
17616  vinfos[0].indices[1] = _ij0[1];
17617  vinfos[0].maxsolutions = _nj0;
17618  vinfos[1].jointtype = 1;
17619  vinfos[1].foffset = j1;
17620  vinfos[1].indices[0] = _ij1[0];
17621  vinfos[1].indices[1] = _ij1[1];
17622  vinfos[1].maxsolutions = _nj1;
17623  vinfos[2].jointtype = 1;
17624  vinfos[2].foffset = j2;
17625  vinfos[2].indices[0] = _ij2[0];
17626  vinfos[2].indices[1] = _ij2[1];
17627  vinfos[2].maxsolutions = _nj2;
17628  vinfos[3].jointtype = 1;
17629  vinfos[3].foffset = j3;
17630  vinfos[3].indices[0] = _ij3[0];
17631  vinfos[3].indices[1] = _ij3[1];
17632  vinfos[3].maxsolutions = _nj3;
17633  vinfos[4].jointtype = 1;
17634  vinfos[4].foffset = j4;
17635  vinfos[4].indices[0] = _ij4[0];
17636  vinfos[4].indices[1] = _ij4[1];
17637  vinfos[4].maxsolutions = _nj4;
17638  vinfos[5].jointtype = 1;
17639  vinfos[5].foffset = j5;
17640  vinfos[5].indices[0] = _ij5[0];
17641  vinfos[5].indices[1] = _ij5[1];
17642  vinfos[5].maxsolutions = _nj5;
17643  vinfos[6].jointtype = 1;
17644  vinfos[6].foffset = j6;
17645  vinfos[6].indices[0] = _ij6[0];
17646  vinfos[6].indices[1] = _ij6[1];
17647  vinfos[6].maxsolutions = _nj6;
17648  std::vector<int> vfree(0);
17649  solutions.AddSolution(vinfos, vfree);
17650  }
17651  }
17652  }
17653  }
17654  }
17655  }
17656  else
17657  {
17658  {
17659  IkReal j0array[1], cj0array[1], sj0array[1];
17660  bool j0valid[1] = { false };
17661  _nj0 = 1;
17662  CheckValue<IkReal> x815 =
17663  IKPowWithIntegerCheck(IKsign(new_r01), -1);
17664  if (!x815.valid)
17665  {
17666  continue;
17667  }
17668  CheckValue<IkReal> x816 =
17669  IKatan2WithCheck(IkReal(cj2),
17670  IkReal(((-1.0) * sj2)),
17671  IKFAST_ATAN2_MAGTHRESH);
17672  if (!x816.valid)
17673  {
17674  continue;
17675  }
17676  j0array[0] =
17677  ((-1.5707963267949) +
17678  (((1.5707963267949) * (x815.value))) +
17679  (x816.value));
17680  sj0array[0] = IKsin(j0array[0]);
17681  cj0array[0] = IKcos(j0array[0]);
17682  if (j0array[0] > IKPI)
17683  {
17684  j0array[0] -= IK2PI;
17685  }
17686  else if (j0array[0] < -IKPI)
17687  {
17688  j0array[0] += IK2PI;
17689  }
17690  j0valid[0] = true;
17691  for (int ij0 = 0; ij0 < 1; ++ij0)
17692  {
17693  if (!j0valid[ij0])
17694  {
17695  continue;
17696  }
17697  _ij0[0] = ij0;
17698  _ij0[1] = -1;
17699  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17700  {
17701  if (j0valid[iij0] &&
17702  IKabs(cj0array[ij0] - cj0array[iij0]) <
17703  IKFAST_SOLUTION_THRESH &&
17704  IKabs(sj0array[ij0] - sj0array[iij0]) <
17705  IKFAST_SOLUTION_THRESH)
17706  {
17707  j0valid[iij0] = false;
17708  _ij0[1] = iij0;
17709  break;
17710  }
17711  }
17712  j0 = j0array[ij0];
17713  cj0 = cj0array[ij0];
17714  sj0 = sj0array[ij0];
17715  {
17716  IkReal evalcond[7];
17717  IkReal x817 = IKcos(j0);
17718  IkReal x818 = IKsin(j0);
17719  IkReal x819 = ((1.0) * cj2);
17720  IkReal x820 = (sj2 * x817);
17721  IkReal x821 = ((1.0) * x818);
17722  IkReal x822 = (x818 * x819);
17723  evalcond[0] = (sj2 + ((new_r10 * x817)));
17724  evalcond[1] = (sj2 + ((new_r01 * x817)));
17725  evalcond[2] =
17726  (cj2 + (((-1.0) * new_r01 * x821)));
17727  evalcond[3] = (((new_r10 * x818)) +
17728  (((-1.0) * x819)));
17729  evalcond[4] =
17730  ((((-1.0) * x822)) + new_r10 + x820);
17731  evalcond[5] = ((((-1.0) * x817 * x819)) +
17732  (((-1.0) * sj2 * x821)));
17733  evalcond[6] =
17734  ((((-1.0) * x822)) + new_r01 + x820);
17735  if (IKabs(evalcond[0]) >
17736  IKFAST_EVALCOND_THRESH ||
17737  IKabs(evalcond[1]) >
17738  IKFAST_EVALCOND_THRESH ||
17739  IKabs(evalcond[2]) >
17740  IKFAST_EVALCOND_THRESH ||
17741  IKabs(evalcond[3]) >
17742  IKFAST_EVALCOND_THRESH ||
17743  IKabs(evalcond[4]) >
17744  IKFAST_EVALCOND_THRESH ||
17745  IKabs(evalcond[5]) >
17746  IKFAST_EVALCOND_THRESH ||
17747  IKabs(evalcond[6]) >
17748  IKFAST_EVALCOND_THRESH)
17749  {
17750  continue;
17751  }
17752  }
17753 
17754  {
17755  std::vector<
17756  IkSingleDOFSolutionBase<IkReal> >
17757  vinfos(7);
17758  vinfos[0].jointtype = 1;
17759  vinfos[0].foffset = j0;
17760  vinfos[0].indices[0] = _ij0[0];
17761  vinfos[0].indices[1] = _ij0[1];
17762  vinfos[0].maxsolutions = _nj0;
17763  vinfos[1].jointtype = 1;
17764  vinfos[1].foffset = j1;
17765  vinfos[1].indices[0] = _ij1[0];
17766  vinfos[1].indices[1] = _ij1[1];
17767  vinfos[1].maxsolutions = _nj1;
17768  vinfos[2].jointtype = 1;
17769  vinfos[2].foffset = j2;
17770  vinfos[2].indices[0] = _ij2[0];
17771  vinfos[2].indices[1] = _ij2[1];
17772  vinfos[2].maxsolutions = _nj2;
17773  vinfos[3].jointtype = 1;
17774  vinfos[3].foffset = j3;
17775  vinfos[3].indices[0] = _ij3[0];
17776  vinfos[3].indices[1] = _ij3[1];
17777  vinfos[3].maxsolutions = _nj3;
17778  vinfos[4].jointtype = 1;
17779  vinfos[4].foffset = j4;
17780  vinfos[4].indices[0] = _ij4[0];
17781  vinfos[4].indices[1] = _ij4[1];
17782  vinfos[4].maxsolutions = _nj4;
17783  vinfos[5].jointtype = 1;
17784  vinfos[5].foffset = j5;
17785  vinfos[5].indices[0] = _ij5[0];
17786  vinfos[5].indices[1] = _ij5[1];
17787  vinfos[5].maxsolutions = _nj5;
17788  vinfos[6].jointtype = 1;
17789  vinfos[6].foffset = j6;
17790  vinfos[6].indices[0] = _ij6[0];
17791  vinfos[6].indices[1] = _ij6[1];
17792  vinfos[6].maxsolutions = _nj6;
17793  std::vector<int> vfree(0);
17794  solutions.AddSolution(vinfos, vfree);
17795  }
17796  }
17797  }
17798  }
17799  }
17800  }
17801  } while (0);
17802  if (bgotonextstatement)
17803  {
17804  bool bgotonextstatement = true;
17805  do
17806  {
17807  evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
17808  if (IKabs(evalcond[0]) < 0.0000050000000000)
17809  {
17810  bgotonextstatement = false;
17811  {
17812  IkReal j0eval[1];
17813  sj1 = 0;
17814  cj1 = -1.0;
17815  j1 = 3.14159265358979;
17816  new_r11 = 0;
17817  new_r01 = 0;
17818  new_r22 = 0;
17819  new_r20 = 0;
17820  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17821  if (IKabs(j0eval[0]) < 0.0000010000000000)
17822  {
17823  continue; // no branches [j0]
17824  }
17825  else
17826  {
17827  {
17828  IkReal j0array[2], cj0array[2], sj0array[2];
17829  bool j0valid[2] = { false };
17830  _nj0 = 2;
17831  CheckValue<IkReal> x824 =
17832  IKatan2WithCheck(IkReal(new_r00),
17833  IkReal(new_r10),
17834  IKFAST_ATAN2_MAGTHRESH);
17835  if (!x824.valid)
17836  {
17837  continue;
17838  }
17839  IkReal x823 = x824.value;
17840  j0array[0] = ((-1.0) * x823);
17841  sj0array[0] = IKsin(j0array[0]);
17842  cj0array[0] = IKcos(j0array[0]);
17843  j0array[1] =
17844  ((3.14159265358979) + (((-1.0) * x823)));
17845  sj0array[1] = IKsin(j0array[1]);
17846  cj0array[1] = IKcos(j0array[1]);
17847  if (j0array[0] > IKPI)
17848  {
17849  j0array[0] -= IK2PI;
17850  }
17851  else if (j0array[0] < -IKPI)
17852  {
17853  j0array[0] += IK2PI;
17854  }
17855  j0valid[0] = true;
17856  if (j0array[1] > IKPI)
17857  {
17858  j0array[1] -= IK2PI;
17859  }
17860  else if (j0array[1] < -IKPI)
17861  {
17862  j0array[1] += IK2PI;
17863  }
17864  j0valid[1] = true;
17865  for (int ij0 = 0; ij0 < 2; ++ij0)
17866  {
17867  if (!j0valid[ij0])
17868  {
17869  continue;
17870  }
17871  _ij0[0] = ij0;
17872  _ij0[1] = -1;
17873  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
17874  {
17875  if (j0valid[iij0] &&
17876  IKabs(cj0array[ij0] -
17877  cj0array[iij0]) <
17878  IKFAST_SOLUTION_THRESH &&
17879  IKabs(sj0array[ij0] -
17880  sj0array[iij0]) <
17881  IKFAST_SOLUTION_THRESH)
17882  {
17883  j0valid[iij0] = false;
17884  _ij0[1] = iij0;
17885  break;
17886  }
17887  }
17888  j0 = j0array[ij0];
17889  cj0 = cj0array[ij0];
17890  sj0 = sj0array[ij0];
17891  {
17892  IkReal evalcond[1];
17893  evalcond[0] =
17894  ((((-1.0) * new_r00 * (IKsin(j0)))) +
17895  ((new_r10 * (IKcos(j0)))));
17896  if (IKabs(evalcond[0]) >
17897  IKFAST_EVALCOND_THRESH)
17898  {
17899  continue;
17900  }
17901  }
17902 
17903  {
17904  std::vector<
17905  IkSingleDOFSolutionBase<IkReal> >
17906  vinfos(7);
17907  vinfos[0].jointtype = 1;
17908  vinfos[0].foffset = j0;
17909  vinfos[0].indices[0] = _ij0[0];
17910  vinfos[0].indices[1] = _ij0[1];
17911  vinfos[0].maxsolutions = _nj0;
17912  vinfos[1].jointtype = 1;
17913  vinfos[1].foffset = j1;
17914  vinfos[1].indices[0] = _ij1[0];
17915  vinfos[1].indices[1] = _ij1[1];
17916  vinfos[1].maxsolutions = _nj1;
17917  vinfos[2].jointtype = 1;
17918  vinfos[2].foffset = j2;
17919  vinfos[2].indices[0] = _ij2[0];
17920  vinfos[2].indices[1] = _ij2[1];
17921  vinfos[2].maxsolutions = _nj2;
17922  vinfos[3].jointtype = 1;
17923  vinfos[3].foffset = j3;
17924  vinfos[3].indices[0] = _ij3[0];
17925  vinfos[3].indices[1] = _ij3[1];
17926  vinfos[3].maxsolutions = _nj3;
17927  vinfos[4].jointtype = 1;
17928  vinfos[4].foffset = j4;
17929  vinfos[4].indices[0] = _ij4[0];
17930  vinfos[4].indices[1] = _ij4[1];
17931  vinfos[4].maxsolutions = _nj4;
17932  vinfos[5].jointtype = 1;
17933  vinfos[5].foffset = j5;
17934  vinfos[5].indices[0] = _ij5[0];
17935  vinfos[5].indices[1] = _ij5[1];
17936  vinfos[5].maxsolutions = _nj5;
17937  vinfos[6].jointtype = 1;
17938  vinfos[6].foffset = j6;
17939  vinfos[6].indices[0] = _ij6[0];
17940  vinfos[6].indices[1] = _ij6[1];
17941  vinfos[6].maxsolutions = _nj6;
17942  std::vector<int> vfree(0);
17943  solutions.AddSolution(vinfos, vfree);
17944  }
17945  }
17946  }
17947  }
17948  }
17949  }
17950  } while (0);
17951  if (bgotonextstatement)
17952  {
17953  bool bgotonextstatement = true;
17954  do
17955  {
17956  evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17957  if (IKabs(evalcond[0]) < 0.0000050000000000)
17958  {
17959  bgotonextstatement = false;
17960  {
17961  IkReal j0eval[1];
17962  sj1 = 0;
17963  cj1 = -1.0;
17964  j1 = 3.14159265358979;
17965  new_r00 = 0;
17966  new_r10 = 0;
17967  new_r21 = 0;
17968  new_r22 = 0;
17969  j0eval[0] =
17970  ((IKabs(new_r11)) + (IKabs(new_r01)));
17971  if (IKabs(j0eval[0]) < 0.0000010000000000)
17972  {
17973  continue; // no branches [j0]
17974  }
17975  else
17976  {
17977  {
17978  IkReal j0array[2], cj0array[2], sj0array[2];
17979  bool j0valid[2] = { false };
17980  _nj0 = 2;
17981  CheckValue<IkReal> x826 = IKatan2WithCheck(
17982  IkReal(new_r01),
17983  IkReal(new_r11),
17984  IKFAST_ATAN2_MAGTHRESH);
17985  if (!x826.valid)
17986  {
17987  continue;
17988  }
17989  IkReal x825 = x826.value;
17990  j0array[0] = ((-1.0) * x825);
17991  sj0array[0] = IKsin(j0array[0]);
17992  cj0array[0] = IKcos(j0array[0]);
17993  j0array[1] = ((3.14159265358979) +
17994  (((-1.0) * x825)));
17995  sj0array[1] = IKsin(j0array[1]);
17996  cj0array[1] = IKcos(j0array[1]);
17997  if (j0array[0] > IKPI)
17998  {
17999  j0array[0] -= IK2PI;
18000  }
18001  else if (j0array[0] < -IKPI)
18002  {
18003  j0array[0] += IK2PI;
18004  }
18005  j0valid[0] = true;
18006  if (j0array[1] > IKPI)
18007  {
18008  j0array[1] -= IK2PI;
18009  }
18010  else if (j0array[1] < -IKPI)
18011  {
18012  j0array[1] += IK2PI;
18013  }
18014  j0valid[1] = true;
18015  for (int ij0 = 0; ij0 < 2; ++ij0)
18016  {
18017  if (!j0valid[ij0])
18018  {
18019  continue;
18020  }
18021  _ij0[0] = ij0;
18022  _ij0[1] = -1;
18023  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18024  {
18025  if (j0valid[iij0] &&
18026  IKabs(cj0array[ij0] -
18027  cj0array[iij0]) <
18028  IKFAST_SOLUTION_THRESH &&
18029  IKabs(sj0array[ij0] -
18030  sj0array[iij0]) <
18031  IKFAST_SOLUTION_THRESH)
18032  {
18033  j0valid[iij0] = false;
18034  _ij0[1] = iij0;
18035  break;
18036  }
18037  }
18038  j0 = j0array[ij0];
18039  cj0 = cj0array[ij0];
18040  sj0 = sj0array[ij0];
18041  {
18042  IkReal evalcond[1];
18043  evalcond[0] =
18044  (((new_r11 * (IKcos(j0)))) +
18045  (((-1.0) * new_r01 *
18046  (IKsin(j0)))));
18047  if (IKabs(evalcond[0]) >
18048  IKFAST_EVALCOND_THRESH)
18049  {
18050  continue;
18051  }
18052  }
18053 
18054  {
18055  std::vector<
18056  IkSingleDOFSolutionBase<IkReal> >
18057  vinfos(7);
18058  vinfos[0].jointtype = 1;
18059  vinfos[0].foffset = j0;
18060  vinfos[0].indices[0] = _ij0[0];
18061  vinfos[0].indices[1] = _ij0[1];
18062  vinfos[0].maxsolutions = _nj0;
18063  vinfos[1].jointtype = 1;
18064  vinfos[1].foffset = j1;
18065  vinfos[1].indices[0] = _ij1[0];
18066  vinfos[1].indices[1] = _ij1[1];
18067  vinfos[1].maxsolutions = _nj1;
18068  vinfos[2].jointtype = 1;
18069  vinfos[2].foffset = j2;
18070  vinfos[2].indices[0] = _ij2[0];
18071  vinfos[2].indices[1] = _ij2[1];
18072  vinfos[2].maxsolutions = _nj2;
18073  vinfos[3].jointtype = 1;
18074  vinfos[3].foffset = j3;
18075  vinfos[3].indices[0] = _ij3[0];
18076  vinfos[3].indices[1] = _ij3[1];
18077  vinfos[3].maxsolutions = _nj3;
18078  vinfos[4].jointtype = 1;
18079  vinfos[4].foffset = j4;
18080  vinfos[4].indices[0] = _ij4[0];
18081  vinfos[4].indices[1] = _ij4[1];
18082  vinfos[4].maxsolutions = _nj4;
18083  vinfos[5].jointtype = 1;
18084  vinfos[5].foffset = j5;
18085  vinfos[5].indices[0] = _ij5[0];
18086  vinfos[5].indices[1] = _ij5[1];
18087  vinfos[5].maxsolutions = _nj5;
18088  vinfos[6].jointtype = 1;
18089  vinfos[6].foffset = j6;
18090  vinfos[6].indices[0] = _ij6[0];
18091  vinfos[6].indices[1] = _ij6[1];
18092  vinfos[6].maxsolutions = _nj6;
18093  std::vector<int> vfree(0);
18094  solutions.AddSolution(vinfos, vfree);
18095  }
18096  }
18097  }
18098  }
18099  }
18100  }
18101  } while (0);
18102  if (bgotonextstatement)
18103  {
18104  bool bgotonextstatement = true;
18105  do
18106  {
18107  evalcond[0] =
18108  ((IKabs(new_r10)) + (IKabs(new_r01)));
18109  if (IKabs(evalcond[0]) < 0.0000050000000000)
18110  {
18111  bgotonextstatement = false;
18112  {
18113  IkReal j0eval[3];
18114  sj1 = 0;
18115  cj1 = -1.0;
18116  j1 = 3.14159265358979;
18117  new_r01 = 0;
18118  new_r10 = 0;
18119  j0eval[0] = new_r11;
18120  j0eval[1] = IKsign(new_r11);
18121  j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
18122  if (IKabs(j0eval[0]) < 0.0000010000000000 ||
18123  IKabs(j0eval[1]) < 0.0000010000000000 ||
18124  IKabs(j0eval[2]) < 0.0000010000000000)
18125  {
18126  {
18127  IkReal j0eval[2];
18128  sj1 = 0;
18129  cj1 = -1.0;
18130  j1 = 3.14159265358979;
18131  new_r01 = 0;
18132  new_r10 = 0;
18133  j0eval[0] = new_r00;
18134  j0eval[1] = new_r11;
18135  if (IKabs(j0eval[0]) <
18136  0.0000010000000000 ||
18137  IKabs(j0eval[1]) < 0.0000010000000000)
18138  {
18139  continue; // no branches [j0]
18140  }
18141  else
18142  {
18143  {
18144  IkReal j0array[1], cj0array[1],
18145  sj0array[1];
18146  bool j0valid[1] = { false };
18147  _nj0 = 1;
18148  CheckValue<IkReal> x827 =
18149  IKPowWithIntegerCheck(new_r00,
18150  -1);
18151  if (!x827.valid)
18152  {
18153  continue;
18154  }
18155  CheckValue<IkReal> x828 =
18156  IKPowWithIntegerCheck(new_r11,
18157  -1);
18158  if (!x828.valid)
18159  {
18160  continue;
18161  }
18162  if (IKabs((sj2 * (x827.value))) <
18163  IKFAST_ATAN2_MAGTHRESH &&
18164  IKabs(((-1.0) * cj2 *
18165  (x828.value))) <
18166  IKFAST_ATAN2_MAGTHRESH &&
18167  IKabs(
18168  IKsqr((sj2 * (x827.value))) +
18169  IKsqr(((-1.0) * cj2 *
18170  (x828.value))) -
18171  1) <= IKFAST_SINCOS_THRESH)
18172  continue;
18173  j0array[0] = IKatan2(
18174  (sj2 * (x827.value)),
18175  ((-1.0) * cj2 * (x828.value)));
18176  sj0array[0] = IKsin(j0array[0]);
18177  cj0array[0] = IKcos(j0array[0]);
18178  if (j0array[0] > IKPI)
18179  {
18180  j0array[0] -= IK2PI;
18181  }
18182  else if (j0array[0] < -IKPI)
18183  {
18184  j0array[0] += IK2PI;
18185  }
18186  j0valid[0] = true;
18187  for (int ij0 = 0; ij0 < 1; ++ij0)
18188  {
18189  if (!j0valid[ij0])
18190  {
18191  continue;
18192  }
18193  _ij0[0] = ij0;
18194  _ij0[1] = -1;
18195  for (int iij0 = ij0 + 1; iij0 < 1;
18196  ++iij0)
18197  {
18198  if (j0valid[iij0] &&
18199  IKabs(cj0array[ij0] -
18200  cj0array[iij0]) <
18201  IKFAST_SOLUTION_THRESH &&
18202  IKabs(sj0array[ij0] -
18203  sj0array[iij0]) <
18204  IKFAST_SOLUTION_THRESH)
18205  {
18206  j0valid[iij0] = false;
18207  _ij0[1] = iij0;
18208  break;
18209  }
18210  }
18211  j0 = j0array[ij0];
18212  cj0 = cj0array[ij0];
18213  sj0 = sj0array[ij0];
18214  {
18215  IkReal evalcond[7];
18216  IkReal x829 = IKsin(j0);
18217  IkReal x830 = IKcos(j0);
18218  IkReal x831 = ((1.0) * cj2);
18219  IkReal x832 = (sj2 * x829);
18220  evalcond[0] =
18221  (((new_r11 * x830)) + cj2);
18222  evalcond[1] =
18223  (sj2 + ((new_r11 * x829)));
18224  evalcond[2] =
18225  (sj2 +
18226  (((-1.0) * new_r00 * x829)));
18227  evalcond[3] =
18228  (((new_r00 * x830)) +
18229  (((-1.0) * x831)));
18230  evalcond[4] =
18231  (((sj2 * x830)) +
18232  (((-1.0) * x829 * x831)));
18233  evalcond[5] = (new_r11 + x832 +
18234  ((cj2 * x830)));
18235  evalcond[6] =
18236  ((((-1.0) * x830 * x831)) +
18237  (((-1.0) * x832)) + new_r00);
18238  if (IKabs(evalcond[0]) >
18239  IKFAST_EVALCOND_THRESH ||
18240  IKabs(evalcond[1]) >
18241  IKFAST_EVALCOND_THRESH ||
18242  IKabs(evalcond[2]) >
18243  IKFAST_EVALCOND_THRESH ||
18244  IKabs(evalcond[3]) >
18245  IKFAST_EVALCOND_THRESH ||
18246  IKabs(evalcond[4]) >
18247  IKFAST_EVALCOND_THRESH ||
18248  IKabs(evalcond[5]) >
18249  IKFAST_EVALCOND_THRESH ||
18250  IKabs(evalcond[6]) >
18251  IKFAST_EVALCOND_THRESH)
18252  {
18253  continue;
18254  }
18255  }
18256 
18257  {
18258  std::vector<
18259  IkSingleDOFSolutionBase<
18260  IkReal> >
18261  vinfos(7);
18262  vinfos[0].jointtype = 1;
18263  vinfos[0].foffset = j0;
18264  vinfos[0].indices[0] = _ij0[0];
18265  vinfos[0].indices[1] = _ij0[1];
18266  vinfos[0].maxsolutions = _nj0;
18267  vinfos[1].jointtype = 1;
18268  vinfos[1].foffset = j1;
18269  vinfos[1].indices[0] = _ij1[0];
18270  vinfos[1].indices[1] = _ij1[1];
18271  vinfos[1].maxsolutions = _nj1;
18272  vinfos[2].jointtype = 1;
18273  vinfos[2].foffset = j2;
18274  vinfos[2].indices[0] = _ij2[0];
18275  vinfos[2].indices[1] = _ij2[1];
18276  vinfos[2].maxsolutions = _nj2;
18277  vinfos[3].jointtype = 1;
18278  vinfos[3].foffset = j3;
18279  vinfos[3].indices[0] = _ij3[0];
18280  vinfos[3].indices[1] = _ij3[1];
18281  vinfos[3].maxsolutions = _nj3;
18282  vinfos[4].jointtype = 1;
18283  vinfos[4].foffset = j4;
18284  vinfos[4].indices[0] = _ij4[0];
18285  vinfos[4].indices[1] = _ij4[1];
18286  vinfos[4].maxsolutions = _nj4;
18287  vinfos[5].jointtype = 1;
18288  vinfos[5].foffset = j5;
18289  vinfos[5].indices[0] = _ij5[0];
18290  vinfos[5].indices[1] = _ij5[1];
18291  vinfos[5].maxsolutions = _nj5;
18292  vinfos[6].jointtype = 1;
18293  vinfos[6].foffset = j6;
18294  vinfos[6].indices[0] = _ij6[0];
18295  vinfos[6].indices[1] = _ij6[1];
18296  vinfos[6].maxsolutions = _nj6;
18297  std::vector<int> vfree(0);
18298  solutions.AddSolution(vinfos,
18299  vfree);
18300  }
18301  }
18302  }
18303  }
18304  }
18305  }
18306  else
18307  {
18308  {
18309  IkReal j0array[1], cj0array[1],
18310  sj0array[1];
18311  bool j0valid[1] = { false };
18312  _nj0 = 1;
18313  CheckValue<IkReal> x833 =
18314  IKPowWithIntegerCheck(IKsign(new_r11),
18315  -1);
18316  if (!x833.valid)
18317  {
18318  continue;
18319  }
18320  CheckValue<IkReal> x834 =
18321  IKatan2WithCheck(
18322  IkReal(((-1.0) * sj2)),
18323  IkReal(((-1.0) * cj2)),
18324  IKFAST_ATAN2_MAGTHRESH);
18325  if (!x834.valid)
18326  {
18327  continue;
18328  }
18329  j0array[0] = ((-1.5707963267949) +
18330  (((1.5707963267949) *
18331  (x833.value))) +
18332  (x834.value));
18333  sj0array[0] = IKsin(j0array[0]);
18334  cj0array[0] = IKcos(j0array[0]);
18335  if (j0array[0] > IKPI)
18336  {
18337  j0array[0] -= IK2PI;
18338  }
18339  else if (j0array[0] < -IKPI)
18340  {
18341  j0array[0] += IK2PI;
18342  }
18343  j0valid[0] = true;
18344  for (int ij0 = 0; ij0 < 1; ++ij0)
18345  {
18346  if (!j0valid[ij0])
18347  {
18348  continue;
18349  }
18350  _ij0[0] = ij0;
18351  _ij0[1] = -1;
18352  for (int iij0 = ij0 + 1; iij0 < 1;
18353  ++iij0)
18354  {
18355  if (j0valid[iij0] &&
18356  IKabs(cj0array[ij0] -
18357  cj0array[iij0]) <
18358  IKFAST_SOLUTION_THRESH &&
18359  IKabs(sj0array[ij0] -
18360  sj0array[iij0]) <
18361  IKFAST_SOLUTION_THRESH)
18362  {
18363  j0valid[iij0] = false;
18364  _ij0[1] = iij0;
18365  break;
18366  }
18367  }
18368  j0 = j0array[ij0];
18369  cj0 = cj0array[ij0];
18370  sj0 = sj0array[ij0];
18371  {
18372  IkReal evalcond[7];
18373  IkReal x835 = IKsin(j0);
18374  IkReal x836 = IKcos(j0);
18375  IkReal x837 = ((1.0) * cj2);
18376  IkReal x838 = (sj2 * x835);
18377  evalcond[0] =
18378  (((new_r11 * x836)) + cj2);
18379  evalcond[1] =
18380  (((new_r11 * x835)) + sj2);
18381  evalcond[2] =
18382  (sj2 +
18383  (((-1.0) * new_r00 * x835)));
18384  evalcond[3] = (((new_r00 * x836)) +
18385  (((-1.0) * x837)));
18386  evalcond[4] =
18387  ((((-1.0) * x835 * x837)) +
18388  ((sj2 * x836)));
18389  evalcond[5] =
18390  (new_r11 + x838 + ((cj2 * x836)));
18391  evalcond[6] =
18392  ((((-1.0) * x836 * x837)) +
18393  (((-1.0) * x838)) + new_r00);
18394  if (IKabs(evalcond[0]) >
18395  IKFAST_EVALCOND_THRESH ||
18396  IKabs(evalcond[1]) >
18397  IKFAST_EVALCOND_THRESH ||
18398  IKabs(evalcond[2]) >
18399  IKFAST_EVALCOND_THRESH ||
18400  IKabs(evalcond[3]) >
18401  IKFAST_EVALCOND_THRESH ||
18402  IKabs(evalcond[4]) >
18403  IKFAST_EVALCOND_THRESH ||
18404  IKabs(evalcond[5]) >
18405  IKFAST_EVALCOND_THRESH ||
18406  IKabs(evalcond[6]) >
18407  IKFAST_EVALCOND_THRESH)
18408  {
18409  continue;
18410  }
18411  }
18412 
18413  {
18414  std::vector<
18415  IkSingleDOFSolutionBase<IkReal> >
18416  vinfos(7);
18417  vinfos[0].jointtype = 1;
18418  vinfos[0].foffset = j0;
18419  vinfos[0].indices[0] = _ij0[0];
18420  vinfos[0].indices[1] = _ij0[1];
18421  vinfos[0].maxsolutions = _nj0;
18422  vinfos[1].jointtype = 1;
18423  vinfos[1].foffset = j1;
18424  vinfos[1].indices[0] = _ij1[0];
18425  vinfos[1].indices[1] = _ij1[1];
18426  vinfos[1].maxsolutions = _nj1;
18427  vinfos[2].jointtype = 1;
18428  vinfos[2].foffset = j2;
18429  vinfos[2].indices[0] = _ij2[0];
18430  vinfos[2].indices[1] = _ij2[1];
18431  vinfos[2].maxsolutions = _nj2;
18432  vinfos[3].jointtype = 1;
18433  vinfos[3].foffset = j3;
18434  vinfos[3].indices[0] = _ij3[0];
18435  vinfos[3].indices[1] = _ij3[1];
18436  vinfos[3].maxsolutions = _nj3;
18437  vinfos[4].jointtype = 1;
18438  vinfos[4].foffset = j4;
18439  vinfos[4].indices[0] = _ij4[0];
18440  vinfos[4].indices[1] = _ij4[1];
18441  vinfos[4].maxsolutions = _nj4;
18442  vinfos[5].jointtype = 1;
18443  vinfos[5].foffset = j5;
18444  vinfos[5].indices[0] = _ij5[0];
18445  vinfos[5].indices[1] = _ij5[1];
18446  vinfos[5].maxsolutions = _nj5;
18447  vinfos[6].jointtype = 1;
18448  vinfos[6].foffset = j6;
18449  vinfos[6].indices[0] = _ij6[0];
18450  vinfos[6].indices[1] = _ij6[1];
18451  vinfos[6].maxsolutions = _nj6;
18452  std::vector<int> vfree(0);
18453  solutions.AddSolution(vinfos, vfree);
18454  }
18455  }
18456  }
18457  }
18458  }
18459  }
18460  } while (0);
18461  if (bgotonextstatement)
18462  {
18463  bool bgotonextstatement = true;
18464  do
18465  {
18466  if (1)
18467  {
18468  bgotonextstatement = false;
18469  continue; // branch miss [j0]
18470  }
18471  } while (0);
18472  if (bgotonextstatement)
18473  {
18474  }
18475  }
18476  }
18477  }
18478  }
18479  }
18480  }
18481  }
18482  }
18483  }
18484  }
18485  }
18486  else
18487  {
18488  {
18489  IkReal j0array[1], cj0array[1], sj0array[1];
18490  bool j0valid[1] = { false };
18491  _nj0 = 1;
18492  CheckValue<IkReal> x839 = IKPowWithIntegerCheck(
18493  IKsign((((new_r11 * sj2)) + ((cj2 * new_r01)))), -1);
18494  if (!x839.valid)
18495  {
18496  continue;
18497  }
18498  CheckValue<IkReal> x840 = IKatan2WithCheck(
18499  IkReal(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10)))),
18500  IkReal(
18501  ((((-1.0) * cj2 * sj2)) + (((-1.0) * new_r10 * new_r11)))),
18502  IKFAST_ATAN2_MAGTHRESH);
18503  if (!x840.valid)
18504  {
18505  continue;
18506  }
18507  j0array[0] = ((-1.5707963267949) +
18508  (((1.5707963267949) * (x839.value))) + (x840.value));
18509  sj0array[0] = IKsin(j0array[0]);
18510  cj0array[0] = IKcos(j0array[0]);
18511  if (j0array[0] > IKPI)
18512  {
18513  j0array[0] -= IK2PI;
18514  }
18515  else if (j0array[0] < -IKPI)
18516  {
18517  j0array[0] += IK2PI;
18518  }
18519  j0valid[0] = true;
18520  for (int ij0 = 0; ij0 < 1; ++ij0)
18521  {
18522  if (!j0valid[ij0])
18523  {
18524  continue;
18525  }
18526  _ij0[0] = ij0;
18527  _ij0[1] = -1;
18528  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18529  {
18530  if (j0valid[iij0] &&
18531  IKabs(cj0array[ij0] - cj0array[iij0]) <
18532  IKFAST_SOLUTION_THRESH &&
18533  IKabs(sj0array[ij0] - sj0array[iij0]) <
18534  IKFAST_SOLUTION_THRESH)
18535  {
18536  j0valid[iij0] = false;
18537  _ij0[1] = iij0;
18538  break;
18539  }
18540  }
18541  j0 = j0array[ij0];
18542  cj0 = cj0array[ij0];
18543  sj0 = sj0array[ij0];
18544  {
18545  IkReal evalcond[8];
18546  IkReal x841 = IKcos(j0);
18547  IkReal x842 = IKsin(j0);
18548  IkReal x843 = ((1.0) * cj2);
18549  IkReal x844 = (sj2 * x841);
18550  IkReal x845 = (sj2 * x842);
18551  IkReal x846 = ((1.0) * x842);
18552  IkReal x847 = (x842 * x843);
18553  evalcond[0] = (sj2 + ((new_r01 * x841)) + ((new_r11 * x842)));
18554  evalcond[1] = (((cj2 * x841)) + new_r11 + x845);
18555  evalcond[2] =
18556  (((new_r10 * x841)) + sj2 + (((-1.0) * new_r00 * x846)));
18557  evalcond[3] =
18558  (cj2 + (((-1.0) * new_r01 * x846)) + ((new_r11 * x841)));
18559  evalcond[4] = ((((-1.0) * x847)) + new_r10 + x844);
18560  evalcond[5] = ((((-1.0) * x847)) + new_r01 + x844);
18561  evalcond[6] = (((new_r10 * x842)) + ((new_r00 * x841)) +
18562  (((-1.0) * x843)));
18563  evalcond[7] =
18564  ((((-1.0) * x841 * x843)) + (((-1.0) * x845)) + new_r00);
18565  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18566  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18567  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18568  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18569  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18570  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18571  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18572  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18573  {
18574  continue;
18575  }
18576  }
18577 
18578  {
18579  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18580  vinfos[0].jointtype = 1;
18581  vinfos[0].foffset = j0;
18582  vinfos[0].indices[0] = _ij0[0];
18583  vinfos[0].indices[1] = _ij0[1];
18584  vinfos[0].maxsolutions = _nj0;
18585  vinfos[1].jointtype = 1;
18586  vinfos[1].foffset = j1;
18587  vinfos[1].indices[0] = _ij1[0];
18588  vinfos[1].indices[1] = _ij1[1];
18589  vinfos[1].maxsolutions = _nj1;
18590  vinfos[2].jointtype = 1;
18591  vinfos[2].foffset = j2;
18592  vinfos[2].indices[0] = _ij2[0];
18593  vinfos[2].indices[1] = _ij2[1];
18594  vinfos[2].maxsolutions = _nj2;
18595  vinfos[3].jointtype = 1;
18596  vinfos[3].foffset = j3;
18597  vinfos[3].indices[0] = _ij3[0];
18598  vinfos[3].indices[1] = _ij3[1];
18599  vinfos[3].maxsolutions = _nj3;
18600  vinfos[4].jointtype = 1;
18601  vinfos[4].foffset = j4;
18602  vinfos[4].indices[0] = _ij4[0];
18603  vinfos[4].indices[1] = _ij4[1];
18604  vinfos[4].maxsolutions = _nj4;
18605  vinfos[5].jointtype = 1;
18606  vinfos[5].foffset = j5;
18607  vinfos[5].indices[0] = _ij5[0];
18608  vinfos[5].indices[1] = _ij5[1];
18609  vinfos[5].maxsolutions = _nj5;
18610  vinfos[6].jointtype = 1;
18611  vinfos[6].foffset = j6;
18612  vinfos[6].indices[0] = _ij6[0];
18613  vinfos[6].indices[1] = _ij6[1];
18614  vinfos[6].maxsolutions = _nj6;
18615  std::vector<int> vfree(0);
18616  solutions.AddSolution(vinfos, vfree);
18617  }
18618  }
18619  }
18620  }
18621  }
18622  }
18623  else
18624  {
18625  {
18626  IkReal j0array[1], cj0array[1], sj0array[1];
18627  bool j0valid[1] = { false };
18628  _nj0 = 1;
18629  IkReal x848 = ((1.0) * new_r11);
18630  CheckValue<IkReal> x849 = IKPowWithIntegerCheck(
18631  IKsign(((new_r01 * new_r01) + (new_r11 * new_r11))), -1);
18632  if (!x849.valid)
18633  {
18634  continue;
18635  }
18636  CheckValue<IkReal> x850 = IKatan2WithCheck(
18637  IkReal(((((-1.0) * sj2 * x848)) + ((cj2 * new_r01)))),
18638  IkReal(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x848)))),
18639  IKFAST_ATAN2_MAGTHRESH);
18640  if (!x850.valid)
18641  {
18642  continue;
18643  }
18644  j0array[0] = ((-1.5707963267949) +
18645  (((1.5707963267949) * (x849.value))) + (x850.value));
18646  sj0array[0] = IKsin(j0array[0]);
18647  cj0array[0] = IKcos(j0array[0]);
18648  if (j0array[0] > IKPI)
18649  {
18650  j0array[0] -= IK2PI;
18651  }
18652  else if (j0array[0] < -IKPI)
18653  {
18654  j0array[0] += IK2PI;
18655  }
18656  j0valid[0] = true;
18657  for (int ij0 = 0; ij0 < 1; ++ij0)
18658  {
18659  if (!j0valid[ij0])
18660  {
18661  continue;
18662  }
18663  _ij0[0] = ij0;
18664  _ij0[1] = -1;
18665  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18666  {
18667  if (j0valid[iij0] &&
18668  IKabs(cj0array[ij0] - cj0array[iij0]) <
18669  IKFAST_SOLUTION_THRESH &&
18670  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18671  {
18672  j0valid[iij0] = false;
18673  _ij0[1] = iij0;
18674  break;
18675  }
18676  }
18677  j0 = j0array[ij0];
18678  cj0 = cj0array[ij0];
18679  sj0 = sj0array[ij0];
18680  {
18681  IkReal evalcond[8];
18682  IkReal x851 = IKcos(j0);
18683  IkReal x852 = IKsin(j0);
18684  IkReal x853 = ((1.0) * cj2);
18685  IkReal x854 = (sj2 * x851);
18686  IkReal x855 = (sj2 * x852);
18687  IkReal x856 = ((1.0) * x852);
18688  IkReal x857 = (x852 * x853);
18689  evalcond[0] = (sj2 + ((new_r11 * x852)) + ((new_r01 * x851)));
18690  evalcond[1] = (((cj2 * x851)) + new_r11 + x855);
18691  evalcond[2] =
18692  (sj2 + (((-1.0) * new_r00 * x856)) + ((new_r10 * x851)));
18693  evalcond[3] =
18694  (cj2 + (((-1.0) * new_r01 * x856)) + ((new_r11 * x851)));
18695  evalcond[4] = ((((-1.0) * x857)) + new_r10 + x854);
18696  evalcond[5] = ((((-1.0) * x857)) + new_r01 + x854);
18697  evalcond[6] =
18698  ((((-1.0) * x853)) + ((new_r10 * x852)) + ((new_r00 * x851)));
18699  evalcond[7] =
18700  ((((-1.0) * x855)) + new_r00 + (((-1.0) * x851 * x853)));
18701  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18702  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18703  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18704  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18705  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18706  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18707  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18708  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18709  {
18710  continue;
18711  }
18712  }
18713 
18714  {
18715  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18716  vinfos[0].jointtype = 1;
18717  vinfos[0].foffset = j0;
18718  vinfos[0].indices[0] = _ij0[0];
18719  vinfos[0].indices[1] = _ij0[1];
18720  vinfos[0].maxsolutions = _nj0;
18721  vinfos[1].jointtype = 1;
18722  vinfos[1].foffset = j1;
18723  vinfos[1].indices[0] = _ij1[0];
18724  vinfos[1].indices[1] = _ij1[1];
18725  vinfos[1].maxsolutions = _nj1;
18726  vinfos[2].jointtype = 1;
18727  vinfos[2].foffset = j2;
18728  vinfos[2].indices[0] = _ij2[0];
18729  vinfos[2].indices[1] = _ij2[1];
18730  vinfos[2].maxsolutions = _nj2;
18731  vinfos[3].jointtype = 1;
18732  vinfos[3].foffset = j3;
18733  vinfos[3].indices[0] = _ij3[0];
18734  vinfos[3].indices[1] = _ij3[1];
18735  vinfos[3].maxsolutions = _nj3;
18736  vinfos[4].jointtype = 1;
18737  vinfos[4].foffset = j4;
18738  vinfos[4].indices[0] = _ij4[0];
18739  vinfos[4].indices[1] = _ij4[1];
18740  vinfos[4].maxsolutions = _nj4;
18741  vinfos[5].jointtype = 1;
18742  vinfos[5].foffset = j5;
18743  vinfos[5].indices[0] = _ij5[0];
18744  vinfos[5].indices[1] = _ij5[1];
18745  vinfos[5].maxsolutions = _nj5;
18746  vinfos[6].jointtype = 1;
18747  vinfos[6].foffset = j6;
18748  vinfos[6].indices[0] = _ij6[0];
18749  vinfos[6].indices[1] = _ij6[1];
18750  vinfos[6].maxsolutions = _nj6;
18751  std::vector<int> vfree(0);
18752  solutions.AddSolution(vinfos, vfree);
18753  }
18754  }
18755  }
18756  }
18757  }
18758  }
18759  else
18760  {
18761  {
18762  IkReal j0array[1], cj0array[1], sj0array[1];
18763  bool j0valid[1] = { false };
18764  _nj0 = 1;
18765  IkReal x858 = ((1.0) * sj2);
18766  CheckValue<IkReal> x859 = IKatan2WithCheck(
18767  IkReal((((new_r01 * sj2)) + (((-1.0) * new_r10 * x858)))),
18768  IkReal(((((-1.0) * new_r00 * x858)) + (((-1.0) * new_r11 * x858)))),
18769  IKFAST_ATAN2_MAGTHRESH);
18770  if (!x859.valid)
18771  {
18772  continue;
18773  }
18774  CheckValue<IkReal> x860 = IKPowWithIntegerCheck(
18775  IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
18776  if (!x860.valid)
18777  {
18778  continue;
18779  }
18780  j0array[0] = ((-1.5707963267949) + (x859.value) +
18781  (((1.5707963267949) * (x860.value))));
18782  sj0array[0] = IKsin(j0array[0]);
18783  cj0array[0] = IKcos(j0array[0]);
18784  if (j0array[0] > IKPI)
18785  {
18786  j0array[0] -= IK2PI;
18787  }
18788  else if (j0array[0] < -IKPI)
18789  {
18790  j0array[0] += IK2PI;
18791  }
18792  j0valid[0] = true;
18793  for (int ij0 = 0; ij0 < 1; ++ij0)
18794  {
18795  if (!j0valid[ij0])
18796  {
18797  continue;
18798  }
18799  _ij0[0] = ij0;
18800  _ij0[1] = -1;
18801  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18802  {
18803  if (j0valid[iij0] &&
18804  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
18805  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18806  {
18807  j0valid[iij0] = false;
18808  _ij0[1] = iij0;
18809  break;
18810  }
18811  }
18812  j0 = j0array[ij0];
18813  cj0 = cj0array[ij0];
18814  sj0 = sj0array[ij0];
18815  {
18816  IkReal evalcond[8];
18817  IkReal x861 = IKcos(j0);
18818  IkReal x862 = IKsin(j0);
18819  IkReal x863 = ((1.0) * cj2);
18820  IkReal x864 = (sj2 * x861);
18821  IkReal x865 = (sj2 * x862);
18822  IkReal x866 = ((1.0) * x862);
18823  IkReal x867 = (x862 * x863);
18824  evalcond[0] = (((new_r01 * x861)) + sj2 + ((new_r11 * x862)));
18825  evalcond[1] = (((cj2 * x861)) + new_r11 + x865);
18826  evalcond[2] = (sj2 + (((-1.0) * new_r00 * x866)) + ((new_r10 * x861)));
18827  evalcond[3] = (cj2 + (((-1.0) * new_r01 * x866)) + ((new_r11 * x861)));
18828  evalcond[4] = ((((-1.0) * x867)) + new_r10 + x864);
18829  evalcond[5] = ((((-1.0) * x867)) + new_r01 + x864);
18830  evalcond[6] =
18831  ((((-1.0) * x863)) + ((new_r00 * x861)) + ((new_r10 * x862)));
18832  evalcond[7] = ((((-1.0) * x865)) + (((-1.0) * x861 * x863)) + new_r00);
18833  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18834  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18835  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18836  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18837  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18838  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18839  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18840  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18841  {
18842  continue;
18843  }
18844  }
18845 
18846  {
18847  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18848  vinfos[0].jointtype = 1;
18849  vinfos[0].foffset = j0;
18850  vinfos[0].indices[0] = _ij0[0];
18851  vinfos[0].indices[1] = _ij0[1];
18852  vinfos[0].maxsolutions = _nj0;
18853  vinfos[1].jointtype = 1;
18854  vinfos[1].foffset = j1;
18855  vinfos[1].indices[0] = _ij1[0];
18856  vinfos[1].indices[1] = _ij1[1];
18857  vinfos[1].maxsolutions = _nj1;
18858  vinfos[2].jointtype = 1;
18859  vinfos[2].foffset = j2;
18860  vinfos[2].indices[0] = _ij2[0];
18861  vinfos[2].indices[1] = _ij2[1];
18862  vinfos[2].maxsolutions = _nj2;
18863  vinfos[3].jointtype = 1;
18864  vinfos[3].foffset = j3;
18865  vinfos[3].indices[0] = _ij3[0];
18866  vinfos[3].indices[1] = _ij3[1];
18867  vinfos[3].maxsolutions = _nj3;
18868  vinfos[4].jointtype = 1;
18869  vinfos[4].foffset = j4;
18870  vinfos[4].indices[0] = _ij4[0];
18871  vinfos[4].indices[1] = _ij4[1];
18872  vinfos[4].maxsolutions = _nj4;
18873  vinfos[5].jointtype = 1;
18874  vinfos[5].foffset = j5;
18875  vinfos[5].indices[0] = _ij5[0];
18876  vinfos[5].indices[1] = _ij5[1];
18877  vinfos[5].maxsolutions = _nj5;
18878  vinfos[6].jointtype = 1;
18879  vinfos[6].foffset = j6;
18880  vinfos[6].indices[0] = _ij6[0];
18881  vinfos[6].indices[1] = _ij6[1];
18882  vinfos[6].maxsolutions = _nj6;
18883  std::vector<int> vfree(0);
18884  solutions.AddSolution(vinfos, vfree);
18885  }
18886  }
18887  }
18888  }
18889  }
18890  }
18891  } while (0);
18892  if (bgotonextstatement)
18893  {
18894  bool bgotonextstatement = true;
18895  do
18896  {
18897  evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
18898  if (IKabs(evalcond[0]) < 0.0000050000000000)
18899  {
18900  bgotonextstatement = false;
18901  {
18902  IkReal j0eval[1];
18903  new_r02 = 0;
18904  new_r12 = 0;
18905  new_r20 = 0;
18906  new_r21 = 0;
18907  j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
18908  if (IKabs(j0eval[0]) < 0.0000010000000000)
18909  {
18910  {
18911  IkReal j0eval[1];
18912  new_r02 = 0;
18913  new_r12 = 0;
18914  new_r20 = 0;
18915  new_r21 = 0;
18916  j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
18917  if (IKabs(j0eval[0]) < 0.0000010000000000)
18918  {
18919  {
18920  IkReal j0eval[1];
18921  new_r02 = 0;
18922  new_r12 = 0;
18923  new_r20 = 0;
18924  new_r21 = 0;
18925  j0eval[0] =
18926  ((IKabs((new_r10 * new_r22))) + (IKabs((new_r00 * new_r22))));
18927  if (IKabs(j0eval[0]) < 0.0000010000000000)
18928  {
18929  continue; // no branches [j0]
18930  }
18931  else
18932  {
18933  {
18934  IkReal j0array[2], cj0array[2], sj0array[2];
18935  bool j0valid[2] = { false };
18936  _nj0 = 2;
18937  CheckValue<IkReal> x869 =
18938  IKatan2WithCheck(IkReal((new_r00 * new_r22)),
18939  IkReal((new_r10 * new_r22)),
18940  IKFAST_ATAN2_MAGTHRESH);
18941  if (!x869.valid)
18942  {
18943  continue;
18944  }
18945  IkReal x868 = x869.value;
18946  j0array[0] = ((-1.0) * x868);
18947  sj0array[0] = IKsin(j0array[0]);
18948  cj0array[0] = IKcos(j0array[0]);
18949  j0array[1] = ((3.14159265358979) + (((-1.0) * x868)));
18950  sj0array[1] = IKsin(j0array[1]);
18951  cj0array[1] = IKcos(j0array[1]);
18952  if (j0array[0] > IKPI)
18953  {
18954  j0array[0] -= IK2PI;
18955  }
18956  else if (j0array[0] < -IKPI)
18957  {
18958  j0array[0] += IK2PI;
18959  }
18960  j0valid[0] = true;
18961  if (j0array[1] > IKPI)
18962  {
18963  j0array[1] -= IK2PI;
18964  }
18965  else if (j0array[1] < -IKPI)
18966  {
18967  j0array[1] += IK2PI;
18968  }
18969  j0valid[1] = true;
18970  for (int ij0 = 0; ij0 < 2; ++ij0)
18971  {
18972  if (!j0valid[ij0])
18973  {
18974  continue;
18975  }
18976  _ij0[0] = ij0;
18977  _ij0[1] = -1;
18978  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18979  {
18980  if (j0valid[iij0] &&
18981  IKabs(cj0array[ij0] - cj0array[iij0]) <
18982  IKFAST_SOLUTION_THRESH &&
18983  IKabs(sj0array[ij0] - sj0array[iij0]) <
18984  IKFAST_SOLUTION_THRESH)
18985  {
18986  j0valid[iij0] = false;
18987  _ij0[1] = iij0;
18988  break;
18989  }
18990  }
18991  j0 = j0array[ij0];
18992  cj0 = cj0array[ij0];
18993  sj0 = sj0array[ij0];
18994  {
18995  IkReal evalcond[5];
18996  IkReal x870 = IKsin(j0);
18997  IkReal x871 = IKcos(j0);
18998  IkReal x872 = ((1.0) * x870);
18999  IkReal x873 = (new_r11 * x870);
19000  IkReal x874 = (new_r01 * x871);
19001  evalcond[0] = (((new_r00 * x871)) + ((new_r10 * x870)));
19002  evalcond[1] = (x873 + x874);
19003  evalcond[2] =
19004  ((((-1.0) * new_r00 * x872)) + ((new_r10 * x871)));
19005  evalcond[3] =
19006  ((((-1.0) * new_r01 * x872)) + ((new_r11 * x871)));
19007  evalcond[4] = (((new_r22 * x874)) + ((new_r22 * x873)));
19008  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19009  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19010  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19011  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19012  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19013  {
19014  continue;
19015  }
19016  }
19017 
19018  {
19019  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19020  vinfos[0].jointtype = 1;
19021  vinfos[0].foffset = j0;
19022  vinfos[0].indices[0] = _ij0[0];
19023  vinfos[0].indices[1] = _ij0[1];
19024  vinfos[0].maxsolutions = _nj0;
19025  vinfos[1].jointtype = 1;
19026  vinfos[1].foffset = j1;
19027  vinfos[1].indices[0] = _ij1[0];
19028  vinfos[1].indices[1] = _ij1[1];
19029  vinfos[1].maxsolutions = _nj1;
19030  vinfos[2].jointtype = 1;
19031  vinfos[2].foffset = j2;
19032  vinfos[2].indices[0] = _ij2[0];
19033  vinfos[2].indices[1] = _ij2[1];
19034  vinfos[2].maxsolutions = _nj2;
19035  vinfos[3].jointtype = 1;
19036  vinfos[3].foffset = j3;
19037  vinfos[3].indices[0] = _ij3[0];
19038  vinfos[3].indices[1] = _ij3[1];
19039  vinfos[3].maxsolutions = _nj3;
19040  vinfos[4].jointtype = 1;
19041  vinfos[4].foffset = j4;
19042  vinfos[4].indices[0] = _ij4[0];
19043  vinfos[4].indices[1] = _ij4[1];
19044  vinfos[4].maxsolutions = _nj4;
19045  vinfos[5].jointtype = 1;
19046  vinfos[5].foffset = j5;
19047  vinfos[5].indices[0] = _ij5[0];
19048  vinfos[5].indices[1] = _ij5[1];
19049  vinfos[5].maxsolutions = _nj5;
19050  vinfos[6].jointtype = 1;
19051  vinfos[6].foffset = j6;
19052  vinfos[6].indices[0] = _ij6[0];
19053  vinfos[6].indices[1] = _ij6[1];
19054  vinfos[6].maxsolutions = _nj6;
19055  std::vector<int> vfree(0);
19056  solutions.AddSolution(vinfos, vfree);
19057  }
19058  }
19059  }
19060  }
19061  }
19062  }
19063  else
19064  {
19065  {
19066  IkReal j0array[2], cj0array[2], sj0array[2];
19067  bool j0valid[2] = { false };
19068  _nj0 = 2;
19069  CheckValue<IkReal> x876 = IKatan2WithCheck(
19070  IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
19071  if (!x876.valid)
19072  {
19073  continue;
19074  }
19075  IkReal x875 = x876.value;
19076  j0array[0] = ((-1.0) * x875);
19077  sj0array[0] = IKsin(j0array[0]);
19078  cj0array[0] = IKcos(j0array[0]);
19079  j0array[1] = ((3.14159265358979) + (((-1.0) * x875)));
19080  sj0array[1] = IKsin(j0array[1]);
19081  cj0array[1] = IKcos(j0array[1]);
19082  if (j0array[0] > IKPI)
19083  {
19084  j0array[0] -= IK2PI;
19085  }
19086  else if (j0array[0] < -IKPI)
19087  {
19088  j0array[0] += IK2PI;
19089  }
19090  j0valid[0] = true;
19091  if (j0array[1] > IKPI)
19092  {
19093  j0array[1] -= IK2PI;
19094  }
19095  else if (j0array[1] < -IKPI)
19096  {
19097  j0array[1] += IK2PI;
19098  }
19099  j0valid[1] = true;
19100  for (int ij0 = 0; ij0 < 2; ++ij0)
19101  {
19102  if (!j0valid[ij0])
19103  {
19104  continue;
19105  }
19106  _ij0[0] = ij0;
19107  _ij0[1] = -1;
19108  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19109  {
19110  if (j0valid[iij0] &&
19111  IKabs(cj0array[ij0] - cj0array[iij0]) <
19112  IKFAST_SOLUTION_THRESH &&
19113  IKabs(sj0array[ij0] - sj0array[iij0]) <
19114  IKFAST_SOLUTION_THRESH)
19115  {
19116  j0valid[iij0] = false;
19117  _ij0[1] = iij0;
19118  break;
19119  }
19120  }
19121  j0 = j0array[ij0];
19122  cj0 = cj0array[ij0];
19123  sj0 = sj0array[ij0];
19124  {
19125  IkReal evalcond[5];
19126  IkReal x877 = IKcos(j0);
19127  IkReal x878 = IKsin(j0);
19128  IkReal x879 = (new_r10 * x878);
19129  IkReal x880 = ((1.0) * x878);
19130  IkReal x881 = (new_r00 * x877);
19131  evalcond[0] = (x879 + x881);
19132  evalcond[1] = ((((-1.0) * new_r00 * x880)) + ((new_r10 * x877)));
19133  evalcond[2] = ((((-1.0) * new_r01 * x880)) + ((new_r11 * x877)));
19134  evalcond[3] = (((new_r22 * x881)) + ((new_r22 * x879)));
19135  evalcond[4] =
19136  (((new_r11 * new_r22 * x878)) + ((new_r01 * new_r22 * x877)));
19137  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19138  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19139  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19140  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19141  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19142  {
19143  continue;
19144  }
19145  }
19146 
19147  {
19148  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19149  vinfos[0].jointtype = 1;
19150  vinfos[0].foffset = j0;
19151  vinfos[0].indices[0] = _ij0[0];
19152  vinfos[0].indices[1] = _ij0[1];
19153  vinfos[0].maxsolutions = _nj0;
19154  vinfos[1].jointtype = 1;
19155  vinfos[1].foffset = j1;
19156  vinfos[1].indices[0] = _ij1[0];
19157  vinfos[1].indices[1] = _ij1[1];
19158  vinfos[1].maxsolutions = _nj1;
19159  vinfos[2].jointtype = 1;
19160  vinfos[2].foffset = j2;
19161  vinfos[2].indices[0] = _ij2[0];
19162  vinfos[2].indices[1] = _ij2[1];
19163  vinfos[2].maxsolutions = _nj2;
19164  vinfos[3].jointtype = 1;
19165  vinfos[3].foffset = j3;
19166  vinfos[3].indices[0] = _ij3[0];
19167  vinfos[3].indices[1] = _ij3[1];
19168  vinfos[3].maxsolutions = _nj3;
19169  vinfos[4].jointtype = 1;
19170  vinfos[4].foffset = j4;
19171  vinfos[4].indices[0] = _ij4[0];
19172  vinfos[4].indices[1] = _ij4[1];
19173  vinfos[4].maxsolutions = _nj4;
19174  vinfos[5].jointtype = 1;
19175  vinfos[5].foffset = j5;
19176  vinfos[5].indices[0] = _ij5[0];
19177  vinfos[5].indices[1] = _ij5[1];
19178  vinfos[5].maxsolutions = _nj5;
19179  vinfos[6].jointtype = 1;
19180  vinfos[6].foffset = j6;
19181  vinfos[6].indices[0] = _ij6[0];
19182  vinfos[6].indices[1] = _ij6[1];
19183  vinfos[6].maxsolutions = _nj6;
19184  std::vector<int> vfree(0);
19185  solutions.AddSolution(vinfos, vfree);
19186  }
19187  }
19188  }
19189  }
19190  }
19191  }
19192  else
19193  {
19194  {
19195  IkReal j0array[2], cj0array[2], sj0array[2];
19196  bool j0valid[2] = { false };
19197  _nj0 = 2;
19198  CheckValue<IkReal> x883 = IKatan2WithCheck(
19199  IkReal(new_r00), IkReal(new_r10), IKFAST_ATAN2_MAGTHRESH);
19200  if (!x883.valid)
19201  {
19202  continue;
19203  }
19204  IkReal x882 = x883.value;
19205  j0array[0] = ((-1.0) * x882);
19206  sj0array[0] = IKsin(j0array[0]);
19207  cj0array[0] = IKcos(j0array[0]);
19208  j0array[1] = ((3.14159265358979) + (((-1.0) * x882)));
19209  sj0array[1] = IKsin(j0array[1]);
19210  cj0array[1] = IKcos(j0array[1]);
19211  if (j0array[0] > IKPI)
19212  {
19213  j0array[0] -= IK2PI;
19214  }
19215  else if (j0array[0] < -IKPI)
19216  {
19217  j0array[0] += IK2PI;
19218  }
19219  j0valid[0] = true;
19220  if (j0array[1] > IKPI)
19221  {
19222  j0array[1] -= IK2PI;
19223  }
19224  else if (j0array[1] < -IKPI)
19225  {
19226  j0array[1] += IK2PI;
19227  }
19228  j0valid[1] = true;
19229  for (int ij0 = 0; ij0 < 2; ++ij0)
19230  {
19231  if (!j0valid[ij0])
19232  {
19233  continue;
19234  }
19235  _ij0[0] = ij0;
19236  _ij0[1] = -1;
19237  for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19238  {
19239  if (j0valid[iij0] &&
19240  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19241  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19242  {
19243  j0valid[iij0] = false;
19244  _ij0[1] = iij0;
19245  break;
19246  }
19247  }
19248  j0 = j0array[ij0];
19249  cj0 = cj0array[ij0];
19250  sj0 = sj0array[ij0];
19251  {
19252  IkReal evalcond[5];
19253  IkReal x884 = IKcos(j0);
19254  IkReal x885 = IKsin(j0);
19255  IkReal x886 = ((1.0) * x885);
19256  IkReal x887 = (new_r11 * x885);
19257  IkReal x888 = (new_r22 * x884);
19258  evalcond[0] = (((new_r01 * x884)) + x887);
19259  evalcond[1] = (((new_r10 * x884)) + (((-1.0) * new_r00 * x886)));
19260  evalcond[2] = (((new_r11 * x884)) + (((-1.0) * new_r01 * x886)));
19261  evalcond[3] = (((new_r00 * x888)) + ((new_r10 * new_r22 * x885)));
19262  evalcond[4] = (((new_r01 * x888)) + ((new_r22 * x887)));
19263  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19264  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19265  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19266  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19267  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19268  {
19269  continue;
19270  }
19271  }
19272 
19273  {
19274  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19275  vinfos[0].jointtype = 1;
19276  vinfos[0].foffset = j0;
19277  vinfos[0].indices[0] = _ij0[0];
19278  vinfos[0].indices[1] = _ij0[1];
19279  vinfos[0].maxsolutions = _nj0;
19280  vinfos[1].jointtype = 1;
19281  vinfos[1].foffset = j1;
19282  vinfos[1].indices[0] = _ij1[0];
19283  vinfos[1].indices[1] = _ij1[1];
19284  vinfos[1].maxsolutions = _nj1;
19285  vinfos[2].jointtype = 1;
19286  vinfos[2].foffset = j2;
19287  vinfos[2].indices[0] = _ij2[0];
19288  vinfos[2].indices[1] = _ij2[1];
19289  vinfos[2].maxsolutions = _nj2;
19290  vinfos[3].jointtype = 1;
19291  vinfos[3].foffset = j3;
19292  vinfos[3].indices[0] = _ij3[0];
19293  vinfos[3].indices[1] = _ij3[1];
19294  vinfos[3].maxsolutions = _nj3;
19295  vinfos[4].jointtype = 1;
19296  vinfos[4].foffset = j4;
19297  vinfos[4].indices[0] = _ij4[0];
19298  vinfos[4].indices[1] = _ij4[1];
19299  vinfos[4].maxsolutions = _nj4;
19300  vinfos[5].jointtype = 1;
19301  vinfos[5].foffset = j5;
19302  vinfos[5].indices[0] = _ij5[0];
19303  vinfos[5].indices[1] = _ij5[1];
19304  vinfos[5].maxsolutions = _nj5;
19305  vinfos[6].jointtype = 1;
19306  vinfos[6].foffset = j6;
19307  vinfos[6].indices[0] = _ij6[0];
19308  vinfos[6].indices[1] = _ij6[1];
19309  vinfos[6].maxsolutions = _nj6;
19310  std::vector<int> vfree(0);
19311  solutions.AddSolution(vinfos, vfree);
19312  }
19313  }
19314  }
19315  }
19316  }
19317  }
19318  } while (0);
19319  if (bgotonextstatement)
19320  {
19321  bool bgotonextstatement = true;
19322  do
19323  {
19324  if (1)
19325  {
19326  bgotonextstatement = false;
19327  continue; // branch miss [j0]
19328  }
19329  } while (0);
19330  if (bgotonextstatement)
19331  {
19332  }
19333  }
19334  }
19335  }
19336  }
19337  }
19338  else
19339  {
19340  {
19341  IkReal j0array[1], cj0array[1], sj0array[1];
19342  bool j0valid[1] = { false };
19343  _nj0 = 1;
19344  CheckValue<IkReal> x890 = IKPowWithIntegerCheck(sj1, -1);
19345  if (!x890.valid)
19346  {
19347  continue;
19348  }
19349  IkReal x889 = x890.value;
19350  CheckValue<IkReal> x891 = IKPowWithIntegerCheck(new_r00, -1);
19351  if (!x891.valid)
19352  {
19353  continue;
19354  }
19355  if (IKabs((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) <
19356  IKFAST_ATAN2_MAGTHRESH &&
19357  IKabs((new_r02 * x889)) < IKFAST_ATAN2_MAGTHRESH &&
19358  IKabs(IKsqr((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) +
19359  IKsqr((new_r02 * x889)) - 1) <= IKFAST_SINCOS_THRESH)
19360  continue;
19361  j0array[0] = IKatan2((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10))))),
19362  (new_r02 * x889));
19363  sj0array[0] = IKsin(j0array[0]);
19364  cj0array[0] = IKcos(j0array[0]);
19365  if (j0array[0] > IKPI)
19366  {
19367  j0array[0] -= IK2PI;
19368  }
19369  else if (j0array[0] < -IKPI)
19370  {
19371  j0array[0] += IK2PI;
19372  }
19373  j0valid[0] = true;
19374  for (int ij0 = 0; ij0 < 1; ++ij0)
19375  {
19376  if (!j0valid[ij0])
19377  {
19378  continue;
19379  }
19380  _ij0[0] = ij0;
19381  _ij0[1] = -1;
19382  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19383  {
19384  if (j0valid[iij0] &&
19385  IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19386  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19387  {
19388  j0valid[iij0] = false;
19389  _ij0[1] = iij0;
19390  break;
19391  }
19392  }
19393  j0 = j0array[ij0];
19394  cj0 = cj0array[ij0];
19395  sj0 = sj0array[ij0];
19396  {
19397  IkReal evalcond[18];
19398  IkReal x892 = IKcos(j0);
19399  IkReal x893 = IKsin(j0);
19400  IkReal x894 = ((1.0) * sj1);
19401  IkReal x895 = ((1.0) * sj2);
19402  IkReal x896 = ((1.0) * cj1);
19403  IkReal x897 = (new_r10 * x893);
19404  IkReal x898 = (cj1 * x892);
19405  IkReal x899 = (cj1 * x893);
19406  IkReal x900 = (new_r00 * x892);
19407  IkReal x901 = ((1.0) * x893);
19408  IkReal x902 = (new_r11 * x893);
19409  IkReal x903 = (new_r12 * x893);
19410  IkReal x904 = (new_r02 * x892);
19411  IkReal x905 = (new_r01 * x892);
19412  evalcond[0] = ((((-1.0) * x892 * x894)) + new_r02);
19413  evalcond[1] = ((((-1.0) * x893 * x894)) + new_r12);
19414  evalcond[2] = ((((-1.0) * new_r02 * x901)) + ((new_r12 * x892)));
19415  evalcond[3] = (sj2 + ((new_r10 * x892)) + (((-1.0) * new_r00 * x901)));
19416  evalcond[4] = (((new_r11 * x892)) + cj2 + (((-1.0) * new_r01 * x901)));
19417  evalcond[5] = (((cj2 * x899)) + ((sj2 * x892)) + new_r10);
19418  evalcond[6] = ((((-1.0) * x894)) + x904 + x903);
19419  evalcond[7] = (((cj1 * cj2)) + x897 + x900);
19420  evalcond[8] = (((cj2 * x898)) + (((-1.0) * x893 * x895)) + new_r00);
19421  evalcond[9] = (((cj2 * x892)) + (((-1.0) * x895 * x899)) + new_r11);
19422  evalcond[10] = ((((-1.0) * cj1 * x895)) + x905 + x902);
19423  evalcond[11] = ((((-1.0) * x895 * x898)) + new_r01 + (((-1.0) * cj2 * x901)));
19424  evalcond[12] =
19425  (((new_r12 * x899)) + ((new_r02 * x898)) + (((-1.0) * new_r22 * x894)));
19426  evalcond[13] =
19427  (cj2 + ((new_r00 * x898)) + (((-1.0) * new_r20 * x894)) + ((cj1 * x897)));
19428  evalcond[14] = ((((-1.0) * x894 * x900)) + (((-1.0) * new_r20 * x896)) +
19429  (((-1.0) * x894 * x897)));
19430  evalcond[15] = ((((-1.0) * x894 * x902)) + (((-1.0) * x894 * x905)) +
19431  (((-1.0) * new_r21 * x896)));
19432  evalcond[16] = ((1.0) + (((-1.0) * x894 * x903)) + (((-1.0) * x894 * x904)) +
19433  (((-1.0) * new_r22 * x896)));
19434  evalcond[17] = (((new_r11 * x899)) + ((new_r01 * x898)) +
19435  (((-1.0) * new_r21 * x894)) + (((-1.0) * x895)));
19436  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19437  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19438  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19439  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19440  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19441  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19442  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19443  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19444  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19445  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19446  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19447  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19448  IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19449  IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19450  IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19451  IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19452  IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19453  IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19454  {
19455  continue;
19456  }
19457  }
19458 
19459  {
19460  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19461  vinfos[0].jointtype = 1;
19462  vinfos[0].foffset = j0;
19463  vinfos[0].indices[0] = _ij0[0];
19464  vinfos[0].indices[1] = _ij0[1];
19465  vinfos[0].maxsolutions = _nj0;
19466  vinfos[1].jointtype = 1;
19467  vinfos[1].foffset = j1;
19468  vinfos[1].indices[0] = _ij1[0];
19469  vinfos[1].indices[1] = _ij1[1];
19470  vinfos[1].maxsolutions = _nj1;
19471  vinfos[2].jointtype = 1;
19472  vinfos[2].foffset = j2;
19473  vinfos[2].indices[0] = _ij2[0];
19474  vinfos[2].indices[1] = _ij2[1];
19475  vinfos[2].maxsolutions = _nj2;
19476  vinfos[3].jointtype = 1;
19477  vinfos[3].foffset = j3;
19478  vinfos[3].indices[0] = _ij3[0];
19479  vinfos[3].indices[1] = _ij3[1];
19480  vinfos[3].maxsolutions = _nj3;
19481  vinfos[4].jointtype = 1;
19482  vinfos[4].foffset = j4;
19483  vinfos[4].indices[0] = _ij4[0];
19484  vinfos[4].indices[1] = _ij4[1];
19485  vinfos[4].maxsolutions = _nj4;
19486  vinfos[5].jointtype = 1;
19487  vinfos[5].foffset = j5;
19488  vinfos[5].indices[0] = _ij5[0];
19489  vinfos[5].indices[1] = _ij5[1];
19490  vinfos[5].maxsolutions = _nj5;
19491  vinfos[6].jointtype = 1;
19492  vinfos[6].foffset = j6;
19493  vinfos[6].indices[0] = _ij6[0];
19494  vinfos[6].indices[1] = _ij6[1];
19495  vinfos[6].maxsolutions = _nj6;
19496  std::vector<int> vfree(0);
19497  solutions.AddSolution(vinfos, vfree);
19498  }
19499  }
19500  }
19501  }
19502  }
19503  }
19504  else
19505  {
19506  {
19507  IkReal j0array[1], cj0array[1], sj0array[1];
19508  bool j0valid[1] = { false };
19509  _nj0 = 1;
19510  CheckValue<IkReal> x906 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19511  if (!x906.valid)
19512  {
19513  continue;
19514  }
19515  CheckValue<IkReal> x907 =
19516  IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19517  if (!x907.valid)
19518  {
19519  continue;
19520  }
19521  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x906.value))) + (x907.value));
19522  sj0array[0] = IKsin(j0array[0]);
19523  cj0array[0] = IKcos(j0array[0]);
19524  if (j0array[0] > IKPI)
19525  {
19526  j0array[0] -= IK2PI;
19527  }
19528  else if (j0array[0] < -IKPI)
19529  {
19530  j0array[0] += IK2PI;
19531  }
19532  j0valid[0] = true;
19533  for (int ij0 = 0; ij0 < 1; ++ij0)
19534  {
19535  if (!j0valid[ij0])
19536  {
19537  continue;
19538  }
19539  _ij0[0] = ij0;
19540  _ij0[1] = -1;
19541  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19542  {
19543  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19544  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19545  {
19546  j0valid[iij0] = false;
19547  _ij0[1] = iij0;
19548  break;
19549  }
19550  }
19551  j0 = j0array[ij0];
19552  cj0 = cj0array[ij0];
19553  sj0 = sj0array[ij0];
19554  {
19555  IkReal evalcond[18];
19556  IkReal x908 = IKcos(j0);
19557  IkReal x909 = IKsin(j0);
19558  IkReal x910 = ((1.0) * sj1);
19559  IkReal x911 = ((1.0) * sj2);
19560  IkReal x912 = ((1.0) * cj1);
19561  IkReal x913 = (new_r10 * x909);
19562  IkReal x914 = (cj1 * x908);
19563  IkReal x915 = (cj1 * x909);
19564  IkReal x916 = (new_r00 * x908);
19565  IkReal x917 = ((1.0) * x909);
19566  IkReal x918 = (new_r11 * x909);
19567  IkReal x919 = (new_r12 * x909);
19568  IkReal x920 = (new_r02 * x908);
19569  IkReal x921 = (new_r01 * x908);
19570  evalcond[0] = ((((-1.0) * x908 * x910)) + new_r02);
19571  evalcond[1] = (new_r12 + (((-1.0) * x909 * x910)));
19572  evalcond[2] = ((((-1.0) * new_r02 * x917)) + ((new_r12 * x908)));
19573  evalcond[3] = (sj2 + ((new_r10 * x908)) + (((-1.0) * new_r00 * x917)));
19574  evalcond[4] = (cj2 + ((new_r11 * x908)) + (((-1.0) * new_r01 * x917)));
19575  evalcond[5] = (((sj2 * x908)) + ((cj2 * x915)) + new_r10);
19576  evalcond[6] = ((((-1.0) * x910)) + x920 + x919);
19577  evalcond[7] = (((cj1 * cj2)) + x913 + x916);
19578  evalcond[8] = (((cj2 * x914)) + new_r00 + (((-1.0) * x909 * x911)));
19579  evalcond[9] = (((cj2 * x908)) + new_r11 + (((-1.0) * x911 * x915)));
19580  evalcond[10] = ((((-1.0) * cj1 * x911)) + x921 + x918);
19581  evalcond[11] = (new_r01 + (((-1.0) * x911 * x914)) + (((-1.0) * cj2 * x917)));
19582  evalcond[12] = ((((-1.0) * new_r22 * x910)) + ((new_r12 * x915)) + ((new_r02 * x914)));
19583  evalcond[13] =
19584  (cj2 + (((-1.0) * new_r20 * x910)) + ((new_r00 * x914)) + ((cj1 * x913)));
19585  evalcond[14] =
19586  ((((-1.0) * new_r20 * x912)) + (((-1.0) * x910 * x913)) + (((-1.0) * x910 * x916)));
19587  evalcond[15] =
19588  ((((-1.0) * new_r21 * x912)) + (((-1.0) * x910 * x918)) + (((-1.0) * x910 * x921)));
19589  evalcond[16] = ((1.0) + (((-1.0) * new_r22 * x912)) + (((-1.0) * x910 * x919)) +
19590  (((-1.0) * x910 * x920)));
19591  evalcond[17] = ((((-1.0) * new_r21 * x910)) + ((new_r11 * x915)) + (((-1.0) * x911)) +
19592  ((new_r01 * x914)));
19593  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19594  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19595  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19596  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19597  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19598  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19599  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19600  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19601  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19602  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19603  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19604  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19605  IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19606  IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19607  IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19608  IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19609  IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19610  IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19611  {
19612  continue;
19613  }
19614  }
19615 
19616  {
19617  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19618  vinfos[0].jointtype = 1;
19619  vinfos[0].foffset = j0;
19620  vinfos[0].indices[0] = _ij0[0];
19621  vinfos[0].indices[1] = _ij0[1];
19622  vinfos[0].maxsolutions = _nj0;
19623  vinfos[1].jointtype = 1;
19624  vinfos[1].foffset = j1;
19625  vinfos[1].indices[0] = _ij1[0];
19626  vinfos[1].indices[1] = _ij1[1];
19627  vinfos[1].maxsolutions = _nj1;
19628  vinfos[2].jointtype = 1;
19629  vinfos[2].foffset = j2;
19630  vinfos[2].indices[0] = _ij2[0];
19631  vinfos[2].indices[1] = _ij2[1];
19632  vinfos[2].maxsolutions = _nj2;
19633  vinfos[3].jointtype = 1;
19634  vinfos[3].foffset = j3;
19635  vinfos[3].indices[0] = _ij3[0];
19636  vinfos[3].indices[1] = _ij3[1];
19637  vinfos[3].maxsolutions = _nj3;
19638  vinfos[4].jointtype = 1;
19639  vinfos[4].foffset = j4;
19640  vinfos[4].indices[0] = _ij4[0];
19641  vinfos[4].indices[1] = _ij4[1];
19642  vinfos[4].maxsolutions = _nj4;
19643  vinfos[5].jointtype = 1;
19644  vinfos[5].foffset = j5;
19645  vinfos[5].indices[0] = _ij5[0];
19646  vinfos[5].indices[1] = _ij5[1];
19647  vinfos[5].maxsolutions = _nj5;
19648  vinfos[6].jointtype = 1;
19649  vinfos[6].foffset = j6;
19650  vinfos[6].indices[0] = _ij6[0];
19651  vinfos[6].indices[1] = _ij6[1];
19652  vinfos[6].maxsolutions = _nj6;
19653  std::vector<int> vfree(0);
19654  solutions.AddSolution(vinfos, vfree);
19655  }
19656  }
19657  }
19658  }
19659  }
19660  }
19661  }
19662  }
19663  }
19664  }
19665  else
19666  {
19667  {
19668  IkReal j0array[1], cj0array[1], sj0array[1];
19669  bool j0valid[1] = { false };
19670  _nj0 = 1;
19671  CheckValue<IkReal> x922 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19672  if (!x922.valid)
19673  {
19674  continue;
19675  }
19676  CheckValue<IkReal> x923 = IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19677  if (!x923.valid)
19678  {
19679  continue;
19680  }
19681  j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x922.value))) + (x923.value));
19682  sj0array[0] = IKsin(j0array[0]);
19683  cj0array[0] = IKcos(j0array[0]);
19684  if (j0array[0] > IKPI)
19685  {
19686  j0array[0] -= IK2PI;
19687  }
19688  else if (j0array[0] < -IKPI)
19689  {
19690  j0array[0] += IK2PI;
19691  }
19692  j0valid[0] = true;
19693  for (int ij0 = 0; ij0 < 1; ++ij0)
19694  {
19695  if (!j0valid[ij0])
19696  {
19697  continue;
19698  }
19699  _ij0[0] = ij0;
19700  _ij0[1] = -1;
19701  for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19702  {
19703  if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19704  IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19705  {
19706  j0valid[iij0] = false;
19707  _ij0[1] = iij0;
19708  break;
19709  }
19710  }
19711  j0 = j0array[ij0];
19712  cj0 = cj0array[ij0];
19713  sj0 = sj0array[ij0];
19714  {
19715  IkReal evalcond[8];
19716  IkReal x924 = IKcos(j0);
19717  IkReal x925 = IKsin(j0);
19718  IkReal x926 = ((1.0) * cj1);
19719  IkReal x927 = ((1.0) * sj1);
19720  IkReal x928 = (new_r12 * x925);
19721  IkReal x929 = (new_r02 * x924);
19722  evalcond[0] = ((((-1.0) * x924 * x927)) + new_r02);
19723  evalcond[1] = ((((-1.0) * x925 * x927)) + new_r12);
19724  evalcond[2] = ((((-1.0) * new_r02 * x925)) + ((new_r12 * x924)));
19725  evalcond[3] = ((((-1.0) * x927)) + x928 + x929);
19726  evalcond[4] = ((((-1.0) * new_r22 * x927)) + ((cj1 * x929)) + ((cj1 * x928)));
19727  evalcond[5] = ((((-1.0) * new_r10 * x925 * x927)) + (((-1.0) * new_r00 * x924 * x927)) +
19728  (((-1.0) * new_r20 * x926)));
19729  evalcond[6] = ((((-1.0) * new_r21 * x926)) + (((-1.0) * new_r01 * x924 * x927)) +
19730  (((-1.0) * new_r11 * x925 * x927)));
19731  evalcond[7] =
19732  ((1.0) + (((-1.0) * x927 * x928)) + (((-1.0) * x927 * x929)) + (((-1.0) * new_r22 * x926)));
19733  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19734  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19735  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19736  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19737  {
19738  continue;
19739  }
19740  }
19741 
19742  {
19743  IkReal j2eval[3];
19744  j2eval[0] = sj1;
19745  j2eval[1] = IKsign(sj1);
19746  j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
19747  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19748  IKabs(j2eval[2]) < 0.0000010000000000)
19749  {
19750  {
19751  IkReal j2eval[2];
19752  j2eval[0] = sj1;
19753  j2eval[1] = cj0;
19754  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
19755  {
19756  {
19757  IkReal j2eval[3];
19758  j2eval[0] = sj1;
19759  j2eval[1] = cj1;
19760  j2eval[2] = sj0;
19761  if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19762  IKabs(j2eval[2]) < 0.0000010000000000)
19763  {
19764  {
19765  IkReal evalcond[5];
19766  bool bgotonextstatement = true;
19767  do
19768  {
19769  evalcond[0] = ((-3.14159265358979) +
19770  (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
19771  evalcond[1] = new_r21;
19772  evalcond[2] = new_r02;
19773  evalcond[3] = new_r12;
19774  evalcond[4] = new_r20;
19775  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19776  IKabs(evalcond[1]) < 0.0000050000000000 &&
19777  IKabs(evalcond[2]) < 0.0000050000000000 &&
19778  IKabs(evalcond[3]) < 0.0000050000000000 &&
19779  IKabs(evalcond[4]) < 0.0000050000000000)
19780  {
19781  bgotonextstatement = false;
19782  {
19783  IkReal j2array[1], cj2array[1], sj2array[1];
19784  bool j2valid[1] = { false };
19785  _nj2 = 1;
19786  IkReal x930 = ((1.0) * cj0);
19787  if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) <
19788  IKFAST_ATAN2_MAGTHRESH &&
19789  IKabs(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) <
19790  IKFAST_ATAN2_MAGTHRESH &&
19791  IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) +
19792  IKsqr(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) -
19793  1) <= IKFAST_SINCOS_THRESH)
19794  continue;
19795  j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930))),
19796  ((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0))));
19797  sj2array[0] = IKsin(j2array[0]);
19798  cj2array[0] = IKcos(j2array[0]);
19799  if (j2array[0] > IKPI)
19800  {
19801  j2array[0] -= IK2PI;
19802  }
19803  else if (j2array[0] < -IKPI)
19804  {
19805  j2array[0] += IK2PI;
19806  }
19807  j2valid[0] = true;
19808  for (int ij2 = 0; ij2 < 1; ++ij2)
19809  {
19810  if (!j2valid[ij2])
19811  {
19812  continue;
19813  }
19814  _ij2[0] = ij2;
19815  _ij2[1] = -1;
19816  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19817  {
19818  if (j2valid[iij2] &&
19819  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19820  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19821  {
19822  j2valid[iij2] = false;
19823  _ij2[1] = iij2;
19824  break;
19825  }
19826  }
19827  j2 = j2array[ij2];
19828  cj2 = cj2array[ij2];
19829  sj2 = sj2array[ij2];
19830  {
19831  IkReal evalcond[8];
19832  IkReal x931 = IKcos(j2);
19833  IkReal x932 = IKsin(j2);
19834  IkReal x933 = ((1.0) * sj0);
19835  IkReal x934 = (cj0 * x931);
19836  IkReal x935 = (cj0 * x932);
19837  IkReal x936 = (x932 * x933);
19838  evalcond[0] = (((new_r10 * sj0)) + ((cj0 * new_r00)) + x931);
19839  evalcond[1] = ((((-1.0) * new_r00 * x933)) + ((cj0 * new_r10)) + x932);
19840  evalcond[2] = ((((-1.0) * new_r01 * x933)) + ((cj0 * new_r11)) + x931);
19841  evalcond[3] = (((sj0 * x931)) + new_r10 + x935);
19842  evalcond[4] = (((new_r11 * sj0)) + (((-1.0) * x932)) + ((cj0 * new_r01)));
19843  evalcond[5] = (new_r00 + x934 + (((-1.0) * x936)));
19844  evalcond[6] = (new_r11 + x934 + (((-1.0) * x936)));
19845  evalcond[7] = ((((-1.0) * x931 * x933)) + (((-1.0) * x935)) + new_r01);
19846  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19847  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19848  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19849  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19850  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19851  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19852  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19853  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19854  {
19855  continue;
19856  }
19857  }
19858 
19859  {
19860  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19861  vinfos[0].jointtype = 1;
19862  vinfos[0].foffset = j0;
19863  vinfos[0].indices[0] = _ij0[0];
19864  vinfos[0].indices[1] = _ij0[1];
19865  vinfos[0].maxsolutions = _nj0;
19866  vinfos[1].jointtype = 1;
19867  vinfos[1].foffset = j1;
19868  vinfos[1].indices[0] = _ij1[0];
19869  vinfos[1].indices[1] = _ij1[1];
19870  vinfos[1].maxsolutions = _nj1;
19871  vinfos[2].jointtype = 1;
19872  vinfos[2].foffset = j2;
19873  vinfos[2].indices[0] = _ij2[0];
19874  vinfos[2].indices[1] = _ij2[1];
19875  vinfos[2].maxsolutions = _nj2;
19876  vinfos[3].jointtype = 1;
19877  vinfos[3].foffset = j3;
19878  vinfos[3].indices[0] = _ij3[0];
19879  vinfos[3].indices[1] = _ij3[1];
19880  vinfos[3].maxsolutions = _nj3;
19881  vinfos[4].jointtype = 1;
19882  vinfos[4].foffset = j4;
19883  vinfos[4].indices[0] = _ij4[0];
19884  vinfos[4].indices[1] = _ij4[1];
19885  vinfos[4].maxsolutions = _nj4;
19886  vinfos[5].jointtype = 1;
19887  vinfos[5].foffset = j5;
19888  vinfos[5].indices[0] = _ij5[0];
19889  vinfos[5].indices[1] = _ij5[1];
19890  vinfos[5].maxsolutions = _nj5;
19891  vinfos[6].jointtype = 1;
19892  vinfos[6].foffset = j6;
19893  vinfos[6].indices[0] = _ij6[0];
19894  vinfos[6].indices[1] = _ij6[1];
19895  vinfos[6].maxsolutions = _nj6;
19896  std::vector<int> vfree(0);
19897  solutions.AddSolution(vinfos, vfree);
19898  }
19899  }
19900  }
19901  }
19902  } while (0);
19903  if (bgotonextstatement)
19904  {
19905  bool bgotonextstatement = true;
19906  do
19907  {
19908  evalcond[0] = ((-3.14159265358979) +
19909  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
19910  6.28318530717959)));
19911  evalcond[1] = new_r21;
19912  evalcond[2] = new_r02;
19913  evalcond[3] = new_r12;
19914  evalcond[4] = new_r20;
19915  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19916  IKabs(evalcond[1]) < 0.0000050000000000 &&
19917  IKabs(evalcond[2]) < 0.0000050000000000 &&
19918  IKabs(evalcond[3]) < 0.0000050000000000 &&
19919  IKabs(evalcond[4]) < 0.0000050000000000)
19920  {
19921  bgotonextstatement = false;
19922  {
19923  IkReal j2array[1], cj2array[1], sj2array[1];
19924  bool j2valid[1] = { false };
19925  _nj2 = 1;
19926  IkReal x937 = ((1.0) * cj0);
19927  if (IKabs(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) <
19928  IKFAST_ATAN2_MAGTHRESH &&
19929  IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) <
19930  IKFAST_ATAN2_MAGTHRESH &&
19931  IKabs(IKsqr(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) +
19932  IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) - 1) <=
19933  IKFAST_SINCOS_THRESH)
19934  continue;
19935  j2array[0] = IKatan2(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0))),
19936  (((new_r10 * sj0)) + (((-1.0) * new_r11 * x937))));
19937  sj2array[0] = IKsin(j2array[0]);
19938  cj2array[0] = IKcos(j2array[0]);
19939  if (j2array[0] > IKPI)
19940  {
19941  j2array[0] -= IK2PI;
19942  }
19943  else if (j2array[0] < -IKPI)
19944  {
19945  j2array[0] += IK2PI;
19946  }
19947  j2valid[0] = true;
19948  for (int ij2 = 0; ij2 < 1; ++ij2)
19949  {
19950  if (!j2valid[ij2])
19951  {
19952  continue;
19953  }
19954  _ij2[0] = ij2;
19955  _ij2[1] = -1;
19956  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19957  {
19958  if (j2valid[iij2] &&
19959  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19960  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19961  {
19962  j2valid[iij2] = false;
19963  _ij2[1] = iij2;
19964  break;
19965  }
19966  }
19967  j2 = j2array[ij2];
19968  cj2 = cj2array[ij2];
19969  sj2 = sj2array[ij2];
19970  {
19971  IkReal evalcond[8];
19972  IkReal x938 = IKsin(j2);
19973  IkReal x939 = IKcos(j2);
19974  IkReal x940 = ((1.0) * sj0);
19975  IkReal x941 = (cj0 * x938);
19976  IkReal x942 = ((1.0) * x939);
19977  IkReal x943 = (x939 * x940);
19978  evalcond[0] = (((new_r11 * sj0)) + ((cj0 * new_r01)) + x938);
19979  evalcond[1] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x940)) + x938);
19980  evalcond[2] = ((((-1.0) * new_r01 * x940)) + ((cj0 * new_r11)) + x939);
19981  evalcond[3] = ((((-1.0) * x942)) + ((new_r10 * sj0)) + ((cj0 * new_r00)));
19982  evalcond[4] = (((sj0 * x938)) + ((cj0 * x939)) + new_r11);
19983  evalcond[5] = ((((-1.0) * x943)) + new_r10 + x941);
19984  evalcond[6] = ((((-1.0) * x943)) + new_r01 + x941);
19985  evalcond[7] =
19986  ((((-1.0) * x938 * x940)) + (((-1.0) * cj0 * x942)) + new_r00);
19987  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19988  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19989  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19990  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19991  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19992  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19993  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19994  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19995  {
19996  continue;
19997  }
19998  }
19999 
20000  {
20001  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20002  vinfos[0].jointtype = 1;
20003  vinfos[0].foffset = j0;
20004  vinfos[0].indices[0] = _ij0[0];
20005  vinfos[0].indices[1] = _ij0[1];
20006  vinfos[0].maxsolutions = _nj0;
20007  vinfos[1].jointtype = 1;
20008  vinfos[1].foffset = j1;
20009  vinfos[1].indices[0] = _ij1[0];
20010  vinfos[1].indices[1] = _ij1[1];
20011  vinfos[1].maxsolutions = _nj1;
20012  vinfos[2].jointtype = 1;
20013  vinfos[2].foffset = j2;
20014  vinfos[2].indices[0] = _ij2[0];
20015  vinfos[2].indices[1] = _ij2[1];
20016  vinfos[2].maxsolutions = _nj2;
20017  vinfos[3].jointtype = 1;
20018  vinfos[3].foffset = j3;
20019  vinfos[3].indices[0] = _ij3[0];
20020  vinfos[3].indices[1] = _ij3[1];
20021  vinfos[3].maxsolutions = _nj3;
20022  vinfos[4].jointtype = 1;
20023  vinfos[4].foffset = j4;
20024  vinfos[4].indices[0] = _ij4[0];
20025  vinfos[4].indices[1] = _ij4[1];
20026  vinfos[4].maxsolutions = _nj4;
20027  vinfos[5].jointtype = 1;
20028  vinfos[5].foffset = j5;
20029  vinfos[5].indices[0] = _ij5[0];
20030  vinfos[5].indices[1] = _ij5[1];
20031  vinfos[5].maxsolutions = _nj5;
20032  vinfos[6].jointtype = 1;
20033  vinfos[6].foffset = j6;
20034  vinfos[6].indices[0] = _ij6[0];
20035  vinfos[6].indices[1] = _ij6[1];
20036  vinfos[6].maxsolutions = _nj6;
20037  std::vector<int> vfree(0);
20038  solutions.AddSolution(vinfos, vfree);
20039  }
20040  }
20041  }
20042  }
20043  } while (0);
20044  if (bgotonextstatement)
20045  {
20046  bool bgotonextstatement = true;
20047  do
20048  {
20049  evalcond[0] = ((-3.14159265358979) +
20050  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
20051  6.28318530717959)));
20052  evalcond[1] = new_r22;
20053  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20054  IKabs(evalcond[1]) < 0.0000050000000000)
20055  {
20056  bgotonextstatement = false;
20057  {
20058  IkReal j2array[1], cj2array[1], sj2array[1];
20059  bool j2valid[1] = { false };
20060  _nj2 = 1;
20061  if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
20062  IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
20063  IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
20064  IKFAST_SINCOS_THRESH)
20065  continue;
20066  j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
20067  sj2array[0] = IKsin(j2array[0]);
20068  cj2array[0] = IKcos(j2array[0]);
20069  if (j2array[0] > IKPI)
20070  {
20071  j2array[0] -= IK2PI;
20072  }
20073  else if (j2array[0] < -IKPI)
20074  {
20075  j2array[0] += IK2PI;
20076  }
20077  j2valid[0] = true;
20078  for (int ij2 = 0; ij2 < 1; ++ij2)
20079  {
20080  if (!j2valid[ij2])
20081  {
20082  continue;
20083  }
20084  _ij2[0] = ij2;
20085  _ij2[1] = -1;
20086  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20087  {
20088  if (j2valid[iij2] &&
20089  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20090  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20091  {
20092  j2valid[iij2] = false;
20093  _ij2[1] = iij2;
20094  break;
20095  }
20096  }
20097  j2 = j2array[ij2];
20098  cj2 = cj2array[ij2];
20099  sj2 = sj2array[ij2];
20100  {
20101  IkReal evalcond[8];
20102  IkReal x944 = IKsin(j2);
20103  IkReal x945 = IKcos(j2);
20104  IkReal x946 = ((1.0) * sj0);
20105  evalcond[0] = (new_r21 + x944);
20106  evalcond[1] = ((((-1.0) * x945)) + new_r20);
20107  evalcond[2] = (((new_r02 * x944)) + new_r10);
20108  evalcond[3] = (((cj0 * x945)) + new_r11);
20109  evalcond[4] = ((((-1.0) * x944 * x946)) + new_r00);
20110  evalcond[5] = ((((-1.0) * x945 * x946)) + new_r01);
20111  evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x946)) + x944);
20112  evalcond[7] = ((((-1.0) * new_r01 * x946)) + ((cj0 * new_r11)) + x945);
20113  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20114  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20115  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20116  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20117  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20118  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20119  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20120  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20121  {
20122  continue;
20123  }
20124  }
20125 
20126  {
20127  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20128  vinfos[0].jointtype = 1;
20129  vinfos[0].foffset = j0;
20130  vinfos[0].indices[0] = _ij0[0];
20131  vinfos[0].indices[1] = _ij0[1];
20132  vinfos[0].maxsolutions = _nj0;
20133  vinfos[1].jointtype = 1;
20134  vinfos[1].foffset = j1;
20135  vinfos[1].indices[0] = _ij1[0];
20136  vinfos[1].indices[1] = _ij1[1];
20137  vinfos[1].maxsolutions = _nj1;
20138  vinfos[2].jointtype = 1;
20139  vinfos[2].foffset = j2;
20140  vinfos[2].indices[0] = _ij2[0];
20141  vinfos[2].indices[1] = _ij2[1];
20142  vinfos[2].maxsolutions = _nj2;
20143  vinfos[3].jointtype = 1;
20144  vinfos[3].foffset = j3;
20145  vinfos[3].indices[0] = _ij3[0];
20146  vinfos[3].indices[1] = _ij3[1];
20147  vinfos[3].maxsolutions = _nj3;
20148  vinfos[4].jointtype = 1;
20149  vinfos[4].foffset = j4;
20150  vinfos[4].indices[0] = _ij4[0];
20151  vinfos[4].indices[1] = _ij4[1];
20152  vinfos[4].maxsolutions = _nj4;
20153  vinfos[5].jointtype = 1;
20154  vinfos[5].foffset = j5;
20155  vinfos[5].indices[0] = _ij5[0];
20156  vinfos[5].indices[1] = _ij5[1];
20157  vinfos[5].maxsolutions = _nj5;
20158  vinfos[6].jointtype = 1;
20159  vinfos[6].foffset = j6;
20160  vinfos[6].indices[0] = _ij6[0];
20161  vinfos[6].indices[1] = _ij6[1];
20162  vinfos[6].maxsolutions = _nj6;
20163  std::vector<int> vfree(0);
20164  solutions.AddSolution(vinfos, vfree);
20165  }
20166  }
20167  }
20168  }
20169  } while (0);
20170  if (bgotonextstatement)
20171  {
20172  bool bgotonextstatement = true;
20173  do
20174  {
20175  evalcond[0] = ((-3.14159265358979) +
20176  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
20177  6.28318530717959)));
20178  evalcond[1] = new_r22;
20179  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20180  IKabs(evalcond[1]) < 0.0000050000000000)
20181  {
20182  bgotonextstatement = false;
20183  {
20184  IkReal j2array[1], cj2array[1], sj2array[1];
20185  bool j2valid[1] = { false };
20186  _nj2 = 1;
20187  if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
20188  IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
20189  IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
20190  IKFAST_SINCOS_THRESH)
20191  continue;
20192  j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
20193  sj2array[0] = IKsin(j2array[0]);
20194  cj2array[0] = IKcos(j2array[0]);
20195  if (j2array[0] > IKPI)
20196  {
20197  j2array[0] -= IK2PI;
20198  }
20199  else if (j2array[0] < -IKPI)
20200  {
20201  j2array[0] += IK2PI;
20202  }
20203  j2valid[0] = true;
20204  for (int ij2 = 0; ij2 < 1; ++ij2)
20205  {
20206  if (!j2valid[ij2])
20207  {
20208  continue;
20209  }
20210  _ij2[0] = ij2;
20211  _ij2[1] = -1;
20212  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20213  {
20214  if (j2valid[iij2] &&
20215  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20216  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20217  {
20218  j2valid[iij2] = false;
20219  _ij2[1] = iij2;
20220  break;
20221  }
20222  }
20223  j2 = j2array[ij2];
20224  cj2 = cj2array[ij2];
20225  sj2 = sj2array[ij2];
20226  {
20227  IkReal evalcond[8];
20228  IkReal x947 = IKcos(j2);
20229  IkReal x948 = IKsin(j2);
20230  IkReal x949 = ((1.0) * sj0);
20231  IkReal x950 = ((1.0) * x948);
20232  evalcond[0] = (new_r20 + x947);
20233  evalcond[1] = (new_r21 + (((-1.0) * x950)));
20234  evalcond[2] = (((cj0 * x947)) + new_r11);
20235  evalcond[3] = (new_r10 + (((-1.0) * new_r02 * x950)));
20236  evalcond[4] = ((((-1.0) * x948 * x949)) + new_r00);
20237  evalcond[5] = ((((-1.0) * x947 * x949)) + new_r01);
20238  evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x949)) + x948);
20239  evalcond[7] = ((((-1.0) * new_r01 * x949)) + ((cj0 * new_r11)) + x947);
20240  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20241  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20242  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20243  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20244  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20245  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20246  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20247  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20248  {
20249  continue;
20250  }
20251  }
20252 
20253  {
20254  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20255  vinfos[0].jointtype = 1;
20256  vinfos[0].foffset = j0;
20257  vinfos[0].indices[0] = _ij0[0];
20258  vinfos[0].indices[1] = _ij0[1];
20259  vinfos[0].maxsolutions = _nj0;
20260  vinfos[1].jointtype = 1;
20261  vinfos[1].foffset = j1;
20262  vinfos[1].indices[0] = _ij1[0];
20263  vinfos[1].indices[1] = _ij1[1];
20264  vinfos[1].maxsolutions = _nj1;
20265  vinfos[2].jointtype = 1;
20266  vinfos[2].foffset = j2;
20267  vinfos[2].indices[0] = _ij2[0];
20268  vinfos[2].indices[1] = _ij2[1];
20269  vinfos[2].maxsolutions = _nj2;
20270  vinfos[3].jointtype = 1;
20271  vinfos[3].foffset = j3;
20272  vinfos[3].indices[0] = _ij3[0];
20273  vinfos[3].indices[1] = _ij3[1];
20274  vinfos[3].maxsolutions = _nj3;
20275  vinfos[4].jointtype = 1;
20276  vinfos[4].foffset = j4;
20277  vinfos[4].indices[0] = _ij4[0];
20278  vinfos[4].indices[1] = _ij4[1];
20279  vinfos[4].maxsolutions = _nj4;
20280  vinfos[5].jointtype = 1;
20281  vinfos[5].foffset = j5;
20282  vinfos[5].indices[0] = _ij5[0];
20283  vinfos[5].indices[1] = _ij5[1];
20284  vinfos[5].maxsolutions = _nj5;
20285  vinfos[6].jointtype = 1;
20286  vinfos[6].foffset = j6;
20287  vinfos[6].indices[0] = _ij6[0];
20288  vinfos[6].indices[1] = _ij6[1];
20289  vinfos[6].maxsolutions = _nj6;
20290  std::vector<int> vfree(0);
20291  solutions.AddSolution(vinfos, vfree);
20292  }
20293  }
20294  }
20295  }
20296  } while (0);
20297  if (bgotonextstatement)
20298  {
20299  bool bgotonextstatement = true;
20300  do
20301  {
20302  evalcond[0] =
20303  ((-3.14159265358979) +
20304  (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
20305  evalcond[1] = new_r12;
20306  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20307  IKabs(evalcond[1]) < 0.0000050000000000)
20308  {
20309  bgotonextstatement = false;
20310  {
20311  IkReal j2array[1], cj2array[1], sj2array[1];
20312  bool j2valid[1] = { false };
20313  _nj2 = 1;
20314  if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
20315  IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
20316  IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) - 1) <=
20317  IKFAST_SINCOS_THRESH)
20318  continue;
20319  j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
20320  sj2array[0] = IKsin(j2array[0]);
20321  cj2array[0] = IKcos(j2array[0]);
20322  if (j2array[0] > IKPI)
20323  {
20324  j2array[0] -= IK2PI;
20325  }
20326  else if (j2array[0] < -IKPI)
20327  {
20328  j2array[0] += IK2PI;
20329  }
20330  j2valid[0] = true;
20331  for (int ij2 = 0; ij2 < 1; ++ij2)
20332  {
20333  if (!j2valid[ij2])
20334  {
20335  continue;
20336  }
20337  _ij2[0] = ij2;
20338  _ij2[1] = -1;
20339  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20340  {
20341  if (j2valid[iij2] &&
20342  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20343  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20344  {
20345  j2valid[iij2] = false;
20346  _ij2[1] = iij2;
20347  break;
20348  }
20349  }
20350  j2 = j2array[ij2];
20351  cj2 = cj2array[ij2];
20352  sj2 = sj2array[ij2];
20353  {
20354  IkReal evalcond[8];
20355  IkReal x951 = IKsin(j2);
20356  IkReal x952 = IKcos(j2);
20357  IkReal x953 = ((1.0) * sj1);
20358  IkReal x954 = ((1.0) * x951);
20359  evalcond[0] = (new_r10 + x951);
20360  evalcond[1] = (new_r11 + x952);
20361  evalcond[2] = (((sj1 * x951)) + new_r21);
20362  evalcond[3] = (((cj1 * x952)) + new_r00);
20363  evalcond[4] = (new_r20 + (((-1.0) * x952 * x953)));
20364  evalcond[5] = ((((-1.0) * cj1 * x954)) + new_r01);
20365  evalcond[6] =
20366  (((cj1 * new_r00)) + x952 + (((-1.0) * new_r20 * x953)));
20367  evalcond[7] = ((((-1.0) * new_r21 * x953)) + ((cj1 * new_r01)) +
20368  (((-1.0) * x954)));
20369  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20370  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20371  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20372  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20373  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20374  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20375  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20376  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20377  {
20378  continue;
20379  }
20380  }
20381 
20382  {
20383  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20384  vinfos[0].jointtype = 1;
20385  vinfos[0].foffset = j0;
20386  vinfos[0].indices[0] = _ij0[0];
20387  vinfos[0].indices[1] = _ij0[1];
20388  vinfos[0].maxsolutions = _nj0;
20389  vinfos[1].jointtype = 1;
20390  vinfos[1].foffset = j1;
20391  vinfos[1].indices[0] = _ij1[0];
20392  vinfos[1].indices[1] = _ij1[1];
20393  vinfos[1].maxsolutions = _nj1;
20394  vinfos[2].jointtype = 1;
20395  vinfos[2].foffset = j2;
20396  vinfos[2].indices[0] = _ij2[0];
20397  vinfos[2].indices[1] = _ij2[1];
20398  vinfos[2].maxsolutions = _nj2;
20399  vinfos[3].jointtype = 1;
20400  vinfos[3].foffset = j3;
20401  vinfos[3].indices[0] = _ij3[0];
20402  vinfos[3].indices[1] = _ij3[1];
20403  vinfos[3].maxsolutions = _nj3;
20404  vinfos[4].jointtype = 1;
20405  vinfos[4].foffset = j4;
20406  vinfos[4].indices[0] = _ij4[0];
20407  vinfos[4].indices[1] = _ij4[1];
20408  vinfos[4].maxsolutions = _nj4;
20409  vinfos[5].jointtype = 1;
20410  vinfos[5].foffset = j5;
20411  vinfos[5].indices[0] = _ij5[0];
20412  vinfos[5].indices[1] = _ij5[1];
20413  vinfos[5].maxsolutions = _nj5;
20414  vinfos[6].jointtype = 1;
20415  vinfos[6].foffset = j6;
20416  vinfos[6].indices[0] = _ij6[0];
20417  vinfos[6].indices[1] = _ij6[1];
20418  vinfos[6].maxsolutions = _nj6;
20419  std::vector<int> vfree(0);
20420  solutions.AddSolution(vinfos, vfree);
20421  }
20422  }
20423  }
20424  }
20425  } while (0);
20426  if (bgotonextstatement)
20427  {
20428  bool bgotonextstatement = true;
20429  do
20430  {
20431  evalcond[0] =
20432  ((-3.14159265358979) +
20433  (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j0)))),
20434  6.28318530717959)));
20435  evalcond[1] = new_r12;
20436  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20437  IKabs(evalcond[1]) < 0.0000050000000000)
20438  {
20439  bgotonextstatement = false;
20440  {
20441  IkReal j2array[1], cj2array[1], sj2array[1];
20442  bool j2valid[1] = { false };
20443  _nj2 = 1;
20444  if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
20445  IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
20446  IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <= IKFAST_SINCOS_THRESH)
20447  continue;
20448  j2array[0] = IKatan2(new_r10, new_r11);
20449  sj2array[0] = IKsin(j2array[0]);
20450  cj2array[0] = IKcos(j2array[0]);
20451  if (j2array[0] > IKPI)
20452  {
20453  j2array[0] -= IK2PI;
20454  }
20455  else if (j2array[0] < -IKPI)
20456  {
20457  j2array[0] += IK2PI;
20458  }
20459  j2valid[0] = true;
20460  for (int ij2 = 0; ij2 < 1; ++ij2)
20461  {
20462  if (!j2valid[ij2])
20463  {
20464  continue;
20465  }
20466  _ij2[0] = ij2;
20467  _ij2[1] = -1;
20468  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20469  {
20470  if (j2valid[iij2] &&
20471  IKabs(cj2array[ij2] - cj2array[iij2]) <
20472  IKFAST_SOLUTION_THRESH &&
20473  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20474  {
20475  j2valid[iij2] = false;
20476  _ij2[1] = iij2;
20477  break;
20478  }
20479  }
20480  j2 = j2array[ij2];
20481  cj2 = cj2array[ij2];
20482  sj2 = sj2array[ij2];
20483  {
20484  IkReal evalcond[8];
20485  IkReal x955 = IKsin(j2);
20486  IkReal x956 = IKcos(j2);
20487  IkReal x957 = ((1.0) * sj1);
20488  IkReal x958 = ((1.0) * new_r00);
20489  IkReal x959 = ((1.0) * new_r01);
20490  IkReal x960 = ((1.0) * x955);
20491  evalcond[0] = (((sj1 * x955)) + new_r21);
20492  evalcond[1] = ((((-1.0) * new_r10)) + x955);
20493  evalcond[2] = ((((-1.0) * new_r11)) + x956);
20494  evalcond[3] = ((((-1.0) * x956 * x957)) + new_r20);
20495  evalcond[4] = (((cj1 * x956)) + (((-1.0) * x958)));
20496  evalcond[5] = ((((-1.0) * cj1 * x960)) + (((-1.0) * x959)));
20497  evalcond[6] =
20498  ((((-1.0) * cj1 * x958)) + x956 + (((-1.0) * new_r20 * x957)));
20499  evalcond[7] = ((((-1.0) * new_r21 * x957)) +
20500  (((-1.0) * cj1 * x959)) + (((-1.0) * x960)));
20501  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20502  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20503  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20504  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20505  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20506  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20507  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20508  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20509  {
20510  continue;
20511  }
20512  }
20513 
20514  {
20515  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20516  vinfos[0].jointtype = 1;
20517  vinfos[0].foffset = j0;
20518  vinfos[0].indices[0] = _ij0[0];
20519  vinfos[0].indices[1] = _ij0[1];
20520  vinfos[0].maxsolutions = _nj0;
20521  vinfos[1].jointtype = 1;
20522  vinfos[1].foffset = j1;
20523  vinfos[1].indices[0] = _ij1[0];
20524  vinfos[1].indices[1] = _ij1[1];
20525  vinfos[1].maxsolutions = _nj1;
20526  vinfos[2].jointtype = 1;
20527  vinfos[2].foffset = j2;
20528  vinfos[2].indices[0] = _ij2[0];
20529  vinfos[2].indices[1] = _ij2[1];
20530  vinfos[2].maxsolutions = _nj2;
20531  vinfos[3].jointtype = 1;
20532  vinfos[3].foffset = j3;
20533  vinfos[3].indices[0] = _ij3[0];
20534  vinfos[3].indices[1] = _ij3[1];
20535  vinfos[3].maxsolutions = _nj3;
20536  vinfos[4].jointtype = 1;
20537  vinfos[4].foffset = j4;
20538  vinfos[4].indices[0] = _ij4[0];
20539  vinfos[4].indices[1] = _ij4[1];
20540  vinfos[4].maxsolutions = _nj4;
20541  vinfos[5].jointtype = 1;
20542  vinfos[5].foffset = j5;
20543  vinfos[5].indices[0] = _ij5[0];
20544  vinfos[5].indices[1] = _ij5[1];
20545  vinfos[5].maxsolutions = _nj5;
20546  vinfos[6].jointtype = 1;
20547  vinfos[6].foffset = j6;
20548  vinfos[6].indices[0] = _ij6[0];
20549  vinfos[6].indices[1] = _ij6[1];
20550  vinfos[6].maxsolutions = _nj6;
20551  std::vector<int> vfree(0);
20552  solutions.AddSolution(vinfos, vfree);
20553  }
20554  }
20555  }
20556  }
20557  } while (0);
20558  if (bgotonextstatement)
20559  {
20560  bool bgotonextstatement = true;
20561  do
20562  {
20563  evalcond[0] =
20564  ((-3.14159265358979) +
20565  (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j0)))),
20566  6.28318530717959)));
20567  evalcond[1] = new_r02;
20568  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20569  IKabs(evalcond[1]) < 0.0000050000000000)
20570  {
20571  bgotonextstatement = false;
20572  {
20573  IkReal j2array[1], cj2array[1], sj2array[1];
20574  bool j2valid[1] = { false };
20575  _nj2 = 1;
20576  if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
20577  IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
20578  IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
20579  IKFAST_SINCOS_THRESH)
20580  continue;
20581  j2array[0] = IKatan2(new_r00, new_r01);
20582  sj2array[0] = IKsin(j2array[0]);
20583  cj2array[0] = IKcos(j2array[0]);
20584  if (j2array[0] > IKPI)
20585  {
20586  j2array[0] -= IK2PI;
20587  }
20588  else if (j2array[0] < -IKPI)
20589  {
20590  j2array[0] += IK2PI;
20591  }
20592  j2valid[0] = true;
20593  for (int ij2 = 0; ij2 < 1; ++ij2)
20594  {
20595  if (!j2valid[ij2])
20596  {
20597  continue;
20598  }
20599  _ij2[0] = ij2;
20600  _ij2[1] = -1;
20601  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20602  {
20603  if (j2valid[iij2] &&
20604  IKabs(cj2array[ij2] - cj2array[iij2]) <
20605  IKFAST_SOLUTION_THRESH &&
20606  IKabs(sj2array[ij2] - sj2array[iij2]) <
20607  IKFAST_SOLUTION_THRESH)
20608  {
20609  j2valid[iij2] = false;
20610  _ij2[1] = iij2;
20611  break;
20612  }
20613  }
20614  j2 = j2array[ij2];
20615  cj2 = cj2array[ij2];
20616  sj2 = sj2array[ij2];
20617  {
20618  IkReal evalcond[8];
20619  IkReal x961 = IKsin(j2);
20620  IkReal x962 = IKcos(j2);
20621  IkReal x963 = ((1.0) * sj1);
20622  IkReal x964 = ((1.0) * x961);
20623  evalcond[0] = (((sj1 * x961)) + new_r21);
20624  evalcond[1] = ((((-1.0) * new_r00)) + x961);
20625  evalcond[2] = ((((-1.0) * new_r01)) + x962);
20626  evalcond[3] = (((cj1 * x962)) + new_r10);
20627  evalcond[4] = ((((-1.0) * x962 * x963)) + new_r20);
20628  evalcond[5] = ((((-1.0) * cj1 * x964)) + new_r11);
20629  evalcond[6] =
20630  (((cj1 * new_r10)) + x962 + (((-1.0) * new_r20 * x963)));
20631  evalcond[7] = ((((-1.0) * new_r21 * x963)) + ((cj1 * new_r11)) +
20632  (((-1.0) * x964)));
20633  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20634  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20635  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20636  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20637  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20638  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20639  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20640  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20641  {
20642  continue;
20643  }
20644  }
20645 
20646  {
20647  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20648  vinfos[0].jointtype = 1;
20649  vinfos[0].foffset = j0;
20650  vinfos[0].indices[0] = _ij0[0];
20651  vinfos[0].indices[1] = _ij0[1];
20652  vinfos[0].maxsolutions = _nj0;
20653  vinfos[1].jointtype = 1;
20654  vinfos[1].foffset = j1;
20655  vinfos[1].indices[0] = _ij1[0];
20656  vinfos[1].indices[1] = _ij1[1];
20657  vinfos[1].maxsolutions = _nj1;
20658  vinfos[2].jointtype = 1;
20659  vinfos[2].foffset = j2;
20660  vinfos[2].indices[0] = _ij2[0];
20661  vinfos[2].indices[1] = _ij2[1];
20662  vinfos[2].maxsolutions = _nj2;
20663  vinfos[3].jointtype = 1;
20664  vinfos[3].foffset = j3;
20665  vinfos[3].indices[0] = _ij3[0];
20666  vinfos[3].indices[1] = _ij3[1];
20667  vinfos[3].maxsolutions = _nj3;
20668  vinfos[4].jointtype = 1;
20669  vinfos[4].foffset = j4;
20670  vinfos[4].indices[0] = _ij4[0];
20671  vinfos[4].indices[1] = _ij4[1];
20672  vinfos[4].maxsolutions = _nj4;
20673  vinfos[5].jointtype = 1;
20674  vinfos[5].foffset = j5;
20675  vinfos[5].indices[0] = _ij5[0];
20676  vinfos[5].indices[1] = _ij5[1];
20677  vinfos[5].maxsolutions = _nj5;
20678  vinfos[6].jointtype = 1;
20679  vinfos[6].foffset = j6;
20680  vinfos[6].indices[0] = _ij6[0];
20681  vinfos[6].indices[1] = _ij6[1];
20682  vinfos[6].maxsolutions = _nj6;
20683  std::vector<int> vfree(0);
20684  solutions.AddSolution(vinfos, vfree);
20685  }
20686  }
20687  }
20688  }
20689  } while (0);
20690  if (bgotonextstatement)
20691  {
20692  bool bgotonextstatement = true;
20693  do
20694  {
20695  evalcond[0] =
20696  ((-3.14159265358979) +
20697  (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j0)))),
20698  6.28318530717959)));
20699  evalcond[1] = new_r02;
20700  if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20701  IKabs(evalcond[1]) < 0.0000050000000000)
20702  {
20703  bgotonextstatement = false;
20704  {
20705  IkReal j2array[1], cj2array[1], sj2array[1];
20706  bool j2valid[1] = { false };
20707  _nj2 = 1;
20708  if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
20709  IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
20710  IKabs(IKsqr(((-1.0) * new_r00)) + IKsqr(((-1.0) * new_r01)) -
20711  1) <= IKFAST_SINCOS_THRESH)
20712  continue;
20713  j2array[0] = IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
20714  sj2array[0] = IKsin(j2array[0]);
20715  cj2array[0] = IKcos(j2array[0]);
20716  if (j2array[0] > IKPI)
20717  {
20718  j2array[0] -= IK2PI;
20719  }
20720  else if (j2array[0] < -IKPI)
20721  {
20722  j2array[0] += IK2PI;
20723  }
20724  j2valid[0] = true;
20725  for (int ij2 = 0; ij2 < 1; ++ij2)
20726  {
20727  if (!j2valid[ij2])
20728  {
20729  continue;
20730  }
20731  _ij2[0] = ij2;
20732  _ij2[1] = -1;
20733  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20734  {
20735  if (j2valid[iij2] &&
20736  IKabs(cj2array[ij2] - cj2array[iij2]) <
20737  IKFAST_SOLUTION_THRESH &&
20738  IKabs(sj2array[ij2] - sj2array[iij2]) <
20739  IKFAST_SOLUTION_THRESH)
20740  {
20741  j2valid[iij2] = false;
20742  _ij2[1] = iij2;
20743  break;
20744  }
20745  }
20746  j2 = j2array[ij2];
20747  cj2 = cj2array[ij2];
20748  sj2 = sj2array[ij2];
20749  {
20750  IkReal evalcond[8];
20751  IkReal x965 = IKsin(j2);
20752  IkReal x966 = IKcos(j2);
20753  IkReal x967 = ((1.0) * new_r11);
20754  IkReal x968 = ((1.0) * sj1);
20755  IkReal x969 = ((1.0) * new_r10);
20756  IkReal x970 = ((1.0) * x965);
20757  evalcond[0] = (new_r00 + x965);
20758  evalcond[1] = (new_r01 + x966);
20759  evalcond[2] = (((sj1 * x965)) + new_r21);
20760  evalcond[3] = ((((-1.0) * x966 * x968)) + new_r20);
20761  evalcond[4] = (((cj1 * x966)) + (((-1.0) * x969)));
20762  evalcond[5] = ((((-1.0) * cj1 * x970)) + (((-1.0) * x967)));
20763  evalcond[6] = ((((-1.0) * cj1 * x969)) + x966 +
20764  (((-1.0) * new_r20 * x968)));
20765  evalcond[7] = ((((-1.0) * cj1 * x967)) +
20766  (((-1.0) * new_r21 * x968)) + (((-1.0) * x970)));
20767  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20768  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20769  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20770  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20771  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20772  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20773  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20774  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20775  {
20776  continue;
20777  }
20778  }
20779 
20780  {
20781  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20782  vinfos[0].jointtype = 1;
20783  vinfos[0].foffset = j0;
20784  vinfos[0].indices[0] = _ij0[0];
20785  vinfos[0].indices[1] = _ij0[1];
20786  vinfos[0].maxsolutions = _nj0;
20787  vinfos[1].jointtype = 1;
20788  vinfos[1].foffset = j1;
20789  vinfos[1].indices[0] = _ij1[0];
20790  vinfos[1].indices[1] = _ij1[1];
20791  vinfos[1].maxsolutions = _nj1;
20792  vinfos[2].jointtype = 1;
20793  vinfos[2].foffset = j2;
20794  vinfos[2].indices[0] = _ij2[0];
20795  vinfos[2].indices[1] = _ij2[1];
20796  vinfos[2].maxsolutions = _nj2;
20797  vinfos[3].jointtype = 1;
20798  vinfos[3].foffset = j3;
20799  vinfos[3].indices[0] = _ij3[0];
20800  vinfos[3].indices[1] = _ij3[1];
20801  vinfos[3].maxsolutions = _nj3;
20802  vinfos[4].jointtype = 1;
20803  vinfos[4].foffset = j4;
20804  vinfos[4].indices[0] = _ij4[0];
20805  vinfos[4].indices[1] = _ij4[1];
20806  vinfos[4].maxsolutions = _nj4;
20807  vinfos[5].jointtype = 1;
20808  vinfos[5].foffset = j5;
20809  vinfos[5].indices[0] = _ij5[0];
20810  vinfos[5].indices[1] = _ij5[1];
20811  vinfos[5].maxsolutions = _nj5;
20812  vinfos[6].jointtype = 1;
20813  vinfos[6].foffset = j6;
20814  vinfos[6].indices[0] = _ij6[0];
20815  vinfos[6].indices[1] = _ij6[1];
20816  vinfos[6].maxsolutions = _nj6;
20817  std::vector<int> vfree(0);
20818  solutions.AddSolution(vinfos, vfree);
20819  }
20820  }
20821  }
20822  }
20823  } while (0);
20824  if (bgotonextstatement)
20825  {
20826  bool bgotonextstatement = true;
20827  do
20828  {
20829  evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
20830  if (IKabs(evalcond[0]) < 0.0000050000000000)
20831  {
20832  bgotonextstatement = false;
20833  {
20834  IkReal j2eval[1];
20835  new_r21 = 0;
20836  new_r20 = 0;
20837  new_r02 = 0;
20838  new_r12 = 0;
20839  j2eval[0] = IKabs(new_r22);
20840  if (IKabs(j2eval[0]) < 0.0000000100000000)
20841  {
20842  continue; // no branches [j2]
20843  }
20844  else
20845  {
20846  IkReal op[2 + 1], zeror[2];
20847  int numroots;
20848  op[0] = ((-1.0) * new_r22);
20849  op[1] = 0;
20850  op[2] = new_r22;
20851  polyroots2(op, zeror, numroots);
20852  IkReal j2array[2], cj2array[2], sj2array[2], tempj2array[1];
20853  int numsolutions = 0;
20854  for (int ij2 = 0; ij2 < numroots; ++ij2)
20855  {
20856  IkReal htj2 = zeror[ij2];
20857  tempj2array[0] = ((2.0) * (atan(htj2)));
20858  for (int kj2 = 0; kj2 < 1; ++kj2)
20859  {
20860  j2array[numsolutions] = tempj2array[kj2];
20861  if (j2array[numsolutions] > IKPI)
20862  {
20863  j2array[numsolutions] -= IK2PI;
20864  }
20865  else if (j2array[numsolutions] < -IKPI)
20866  {
20867  j2array[numsolutions] += IK2PI;
20868  }
20869  sj2array[numsolutions] = IKsin(j2array[numsolutions]);
20870  cj2array[numsolutions] = IKcos(j2array[numsolutions]);
20871  numsolutions++;
20872  }
20873  }
20874  bool j2valid[2] = { true, true };
20875  _nj2 = 2;
20876  for (int ij2 = 0; ij2 < numsolutions; ++ij2)
20877  {
20878  if (!j2valid[ij2])
20879  {
20880  continue;
20881  }
20882  j2 = j2array[ij2];
20883  cj2 = cj2array[ij2];
20884  sj2 = sj2array[ij2];
20885  htj2 = IKtan(j2 / 2);
20886 
20887  _ij2[0] = ij2;
20888  _ij2[1] = -1;
20889  for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
20890  {
20891  if (j2valid[iij2] &&
20892  IKabs(cj2array[ij2] - cj2array[iij2]) <
20893  IKFAST_SOLUTION_THRESH &&
20894  IKabs(sj2array[ij2] - sj2array[iij2]) <
20895  IKFAST_SOLUTION_THRESH)
20896  {
20897  j2valid[iij2] = false;
20898  _ij2[1] = iij2;
20899  break;
20900  }
20901  }
20902  {
20903  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20904  vinfos[0].jointtype = 1;
20905  vinfos[0].foffset = j0;
20906  vinfos[0].indices[0] = _ij0[0];
20907  vinfos[0].indices[1] = _ij0[1];
20908  vinfos[0].maxsolutions = _nj0;
20909  vinfos[1].jointtype = 1;
20910  vinfos[1].foffset = j1;
20911  vinfos[1].indices[0] = _ij1[0];
20912  vinfos[1].indices[1] = _ij1[1];
20913  vinfos[1].maxsolutions = _nj1;
20914  vinfos[2].jointtype = 1;
20915  vinfos[2].foffset = j2;
20916  vinfos[2].indices[0] = _ij2[0];
20917  vinfos[2].indices[1] = _ij2[1];
20918  vinfos[2].maxsolutions = _nj2;
20919  vinfos[3].jointtype = 1;
20920  vinfos[3].foffset = j3;
20921  vinfos[3].indices[0] = _ij3[0];
20922  vinfos[3].indices[1] = _ij3[1];
20923  vinfos[3].maxsolutions = _nj3;
20924  vinfos[4].jointtype = 1;
20925  vinfos[4].foffset = j4;
20926  vinfos[4].indices[0] = _ij4[0];
20927  vinfos[4].indices[1] = _ij4[1];
20928  vinfos[4].maxsolutions = _nj4;
20929  vinfos[5].jointtype = 1;
20930  vinfos[5].foffset = j5;
20931  vinfos[5].indices[0] = _ij5[0];
20932  vinfos[5].indices[1] = _ij5[1];
20933  vinfos[5].maxsolutions = _nj5;
20934  vinfos[6].jointtype = 1;
20935  vinfos[6].foffset = j6;
20936  vinfos[6].indices[0] = _ij6[0];
20937  vinfos[6].indices[1] = _ij6[1];
20938  vinfos[6].maxsolutions = _nj6;
20939  std::vector<int> vfree(0);
20940  solutions.AddSolution(vinfos, vfree);
20941  }
20942  }
20943  }
20944  }
20945  }
20946  } while (0);
20947  if (bgotonextstatement)
20948  {
20949  bool bgotonextstatement = true;
20950  do
20951  {
20952  if (1)
20953  {
20954  bgotonextstatement = false;
20955  continue; // branch miss [j2]
20956  }
20957  } while (0);
20958  if (bgotonextstatement)
20959  {
20960  }
20961  }
20962  }
20963  }
20964  }
20965  }
20966  }
20967  }
20968  }
20969  }
20970  }
20971  }
20972  else
20973  {
20974  {
20975  IkReal j2array[1], cj2array[1], sj2array[1];
20976  bool j2valid[1] = { false };
20977  _nj2 = 1;
20978  CheckValue<IkReal> x972 = IKPowWithIntegerCheck(sj1, -1);
20979  if (!x972.valid)
20980  {
20981  continue;
20982  }
20983  IkReal x971 = x972.value;
20984  CheckValue<IkReal> x973 = IKPowWithIntegerCheck(cj1, -1);
20985  if (!x973.valid)
20986  {
20987  continue;
20988  }
20989  CheckValue<IkReal> x974 = IKPowWithIntegerCheck(sj0, -1);
20990  if (!x974.valid)
20991  {
20992  continue;
20993  }
20994  if (IKabs(((-1.0) * new_r21 * x971)) < IKFAST_ATAN2_MAGTHRESH &&
20995  IKabs((x971 * (x973.value) * (x974.value) *
20996  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
20997  IKFAST_ATAN2_MAGTHRESH &&
20998  IKabs(IKsqr(((-1.0) * new_r21 * x971)) +
20999  IKsqr((x971 * (x973.value) * (x974.value) *
21000  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
21001  1) <= IKFAST_SINCOS_THRESH)
21002  continue;
21003  j2array[0] = IKatan2(((-1.0) * new_r21 * x971),
21004  (x971 * (x973.value) * (x974.value) *
21005  ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
21006  sj2array[0] = IKsin(j2array[0]);
21007  cj2array[0] = IKcos(j2array[0]);
21008  if (j2array[0] > IKPI)
21009  {
21010  j2array[0] -= IK2PI;
21011  }
21012  else if (j2array[0] < -IKPI)
21013  {
21014  j2array[0] += IK2PI;
21015  }
21016  j2valid[0] = true;
21017  for (int ij2 = 0; ij2 < 1; ++ij2)
21018  {
21019  if (!j2valid[ij2])
21020  {
21021  continue;
21022  }
21023  _ij2[0] = ij2;
21024  _ij2[1] = -1;
21025  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21026  {
21027  if (j2valid[iij2] &&
21028  IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21029  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21030  {
21031  j2valid[iij2] = false;
21032  _ij2[1] = iij2;
21033  break;
21034  }
21035  }
21036  j2 = j2array[ij2];
21037  cj2 = cj2array[ij2];
21038  sj2 = sj2array[ij2];
21039  {
21040  IkReal evalcond[12];
21041  IkReal x975 = IKsin(j2);
21042  IkReal x976 = IKcos(j2);
21043  IkReal x977 = ((1.0) * sj0);
21044  IkReal x978 = ((1.0) * sj1);
21045  IkReal x979 = (cj0 * new_r00);
21046  IkReal x980 = (cj1 * sj0);
21047  IkReal x981 = (cj0 * new_r01);
21048  IkReal x982 = (cj1 * x976);
21049  IkReal x983 = (cj1 * x975);
21050  evalcond[0] = (((sj1 * x975)) + new_r21);
21051  evalcond[1] = ((((-1.0) * x976 * x978)) + new_r20);
21052  evalcond[2] = ((((-1.0) * new_r00 * x977)) + ((cj0 * new_r10)) + x975);
21053  evalcond[3] = ((((-1.0) * new_r01 * x977)) + ((cj0 * new_r11)) + x976);
21054  evalcond[4] = (((new_r10 * sj0)) + x982 + x979);
21055  evalcond[5] = (((x976 * x980)) + new_r10 + ((cj0 * x975)));
21056  evalcond[6] = ((((-1.0) * x983)) + ((new_r11 * sj0)) + x981);
21057  evalcond[7] = (new_r00 + ((cj0 * x982)) + (((-1.0) * x975 * x977)));
21058  evalcond[8] = ((((-1.0) * x977 * x983)) + new_r11 + ((cj0 * x976)));
21059  evalcond[9] = ((((-1.0) * x976 * x977)) + new_r01 + (((-1.0) * cj0 * x983)));
21060  evalcond[10] =
21061  ((((-1.0) * new_r20 * x978)) + ((cj1 * x979)) + ((new_r10 * x980)) + x976);
21062  evalcond[11] = (((cj1 * x981)) + (((-1.0) * x975)) + (((-1.0) * new_r21 * x978)) +
21063  ((new_r11 * x980)));
21064  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21065  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21066  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21067  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21068  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21069  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21070  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21071  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21072  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21073  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21074  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21075  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21076  {
21077  continue;
21078  }
21079  }
21080 
21081  {
21082  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21083  vinfos[0].jointtype = 1;
21084  vinfos[0].foffset = j0;
21085  vinfos[0].indices[0] = _ij0[0];
21086  vinfos[0].indices[1] = _ij0[1];
21087  vinfos[0].maxsolutions = _nj0;
21088  vinfos[1].jointtype = 1;
21089  vinfos[1].foffset = j1;
21090  vinfos[1].indices[0] = _ij1[0];
21091  vinfos[1].indices[1] = _ij1[1];
21092  vinfos[1].maxsolutions = _nj1;
21093  vinfos[2].jointtype = 1;
21094  vinfos[2].foffset = j2;
21095  vinfos[2].indices[0] = _ij2[0];
21096  vinfos[2].indices[1] = _ij2[1];
21097  vinfos[2].maxsolutions = _nj2;
21098  vinfos[3].jointtype = 1;
21099  vinfos[3].foffset = j3;
21100  vinfos[3].indices[0] = _ij3[0];
21101  vinfos[3].indices[1] = _ij3[1];
21102  vinfos[3].maxsolutions = _nj3;
21103  vinfos[4].jointtype = 1;
21104  vinfos[4].foffset = j4;
21105  vinfos[4].indices[0] = _ij4[0];
21106  vinfos[4].indices[1] = _ij4[1];
21107  vinfos[4].maxsolutions = _nj4;
21108  vinfos[5].jointtype = 1;
21109  vinfos[5].foffset = j5;
21110  vinfos[5].indices[0] = _ij5[0];
21111  vinfos[5].indices[1] = _ij5[1];
21112  vinfos[5].maxsolutions = _nj5;
21113  vinfos[6].jointtype = 1;
21114  vinfos[6].foffset = j6;
21115  vinfos[6].indices[0] = _ij6[0];
21116  vinfos[6].indices[1] = _ij6[1];
21117  vinfos[6].maxsolutions = _nj6;
21118  std::vector<int> vfree(0);
21119  solutions.AddSolution(vinfos, vfree);
21120  }
21121  }
21122  }
21123  }
21124  }
21125  }
21126  else
21127  {
21128  {
21129  IkReal j2array[1], cj2array[1], sj2array[1];
21130  bool j2valid[1] = { false };
21131  _nj2 = 1;
21132  CheckValue<IkReal> x985 = IKPowWithIntegerCheck(sj1, -1);
21133  if (!x985.valid)
21134  {
21135  continue;
21136  }
21137  IkReal x984 = x985.value;
21138  CheckValue<IkReal> x986 = IKPowWithIntegerCheck(cj0, -1);
21139  if (!x986.valid)
21140  {
21141  continue;
21142  }
21143  if (IKabs(((-1.0) * new_r21 * x984)) < IKFAST_ATAN2_MAGTHRESH &&
21144  IKabs((x984 * (x986.value) *
21145  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
21146  IKFAST_ATAN2_MAGTHRESH &&
21147  IKabs(IKsqr(((-1.0) * new_r21 * x984)) +
21148  IKsqr((x984 * (x986.value) *
21149  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
21150  1) <= IKFAST_SINCOS_THRESH)
21151  continue;
21152  j2array[0] = IKatan2(((-1.0) * new_r21 * x984),
21153  (x984 * (x986.value) *
21154  (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
21155  sj2array[0] = IKsin(j2array[0]);
21156  cj2array[0] = IKcos(j2array[0]);
21157  if (j2array[0] > IKPI)
21158  {
21159  j2array[0] -= IK2PI;
21160  }
21161  else if (j2array[0] < -IKPI)
21162  {
21163  j2array[0] += IK2PI;
21164  }
21165  j2valid[0] = true;
21166  for (int ij2 = 0; ij2 < 1; ++ij2)
21167  {
21168  if (!j2valid[ij2])
21169  {
21170  continue;
21171  }
21172  _ij2[0] = ij2;
21173  _ij2[1] = -1;
21174  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21175  {
21176  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21177  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21178  {
21179  j2valid[iij2] = false;
21180  _ij2[1] = iij2;
21181  break;
21182  }
21183  }
21184  j2 = j2array[ij2];
21185  cj2 = cj2array[ij2];
21186  sj2 = sj2array[ij2];
21187  {
21188  IkReal evalcond[12];
21189  IkReal x987 = IKsin(j2);
21190  IkReal x988 = IKcos(j2);
21191  IkReal x989 = ((1.0) * sj0);
21192  IkReal x990 = ((1.0) * sj1);
21193  IkReal x991 = (cj0 * new_r00);
21194  IkReal x992 = (cj1 * sj0);
21195  IkReal x993 = (cj0 * new_r01);
21196  IkReal x994 = (cj1 * x988);
21197  IkReal x995 = (cj1 * x987);
21198  evalcond[0] = (((sj1 * x987)) + new_r21);
21199  evalcond[1] = (new_r20 + (((-1.0) * x988 * x990)));
21200  evalcond[2] = ((((-1.0) * new_r00 * x989)) + ((cj0 * new_r10)) + x987);
21201  evalcond[3] = ((((-1.0) * new_r01 * x989)) + ((cj0 * new_r11)) + x988);
21202  evalcond[4] = (((new_r10 * sj0)) + x991 + x994);
21203  evalcond[5] = (((x988 * x992)) + new_r10 + ((cj0 * x987)));
21204  evalcond[6] = ((((-1.0) * x995)) + ((new_r11 * sj0)) + x993);
21205  evalcond[7] = (((cj0 * x994)) + (((-1.0) * x987 * x989)) + new_r00);
21206  evalcond[8] = (new_r11 + (((-1.0) * x989 * x995)) + ((cj0 * x988)));
21207  evalcond[9] = ((((-1.0) * x988 * x989)) + (((-1.0) * cj0 * x995)) + new_r01);
21208  evalcond[10] =
21209  (((cj1 * x991)) + ((new_r10 * x992)) + (((-1.0) * new_r20 * x990)) + x988);
21210  evalcond[11] = (((cj1 * x993)) + (((-1.0) * x987)) + ((new_r11 * x992)) +
21211  (((-1.0) * new_r21 * x990)));
21212  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21213  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21214  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21215  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21216  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21217  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21218  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21219  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21220  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21221  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21222  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21223  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21224  {
21225  continue;
21226  }
21227  }
21228 
21229  {
21230  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21231  vinfos[0].jointtype = 1;
21232  vinfos[0].foffset = j0;
21233  vinfos[0].indices[0] = _ij0[0];
21234  vinfos[0].indices[1] = _ij0[1];
21235  vinfos[0].maxsolutions = _nj0;
21236  vinfos[1].jointtype = 1;
21237  vinfos[1].foffset = j1;
21238  vinfos[1].indices[0] = _ij1[0];
21239  vinfos[1].indices[1] = _ij1[1];
21240  vinfos[1].maxsolutions = _nj1;
21241  vinfos[2].jointtype = 1;
21242  vinfos[2].foffset = j2;
21243  vinfos[2].indices[0] = _ij2[0];
21244  vinfos[2].indices[1] = _ij2[1];
21245  vinfos[2].maxsolutions = _nj2;
21246  vinfos[3].jointtype = 1;
21247  vinfos[3].foffset = j3;
21248  vinfos[3].indices[0] = _ij3[0];
21249  vinfos[3].indices[1] = _ij3[1];
21250  vinfos[3].maxsolutions = _nj3;
21251  vinfos[4].jointtype = 1;
21252  vinfos[4].foffset = j4;
21253  vinfos[4].indices[0] = _ij4[0];
21254  vinfos[4].indices[1] = _ij4[1];
21255  vinfos[4].maxsolutions = _nj4;
21256  vinfos[5].jointtype = 1;
21257  vinfos[5].foffset = j5;
21258  vinfos[5].indices[0] = _ij5[0];
21259  vinfos[5].indices[1] = _ij5[1];
21260  vinfos[5].maxsolutions = _nj5;
21261  vinfos[6].jointtype = 1;
21262  vinfos[6].foffset = j6;
21263  vinfos[6].indices[0] = _ij6[0];
21264  vinfos[6].indices[1] = _ij6[1];
21265  vinfos[6].maxsolutions = _nj6;
21266  std::vector<int> vfree(0);
21267  solutions.AddSolution(vinfos, vfree);
21268  }
21269  }
21270  }
21271  }
21272  }
21273  }
21274  else
21275  {
21276  {
21277  IkReal j2array[1], cj2array[1], sj2array[1];
21278  bool j2valid[1] = { false };
21279  _nj2 = 1;
21280  CheckValue<IkReal> x996 =
21281  IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
21282  if (!x996.valid)
21283  {
21284  continue;
21285  }
21286  CheckValue<IkReal> x997 = IKPowWithIntegerCheck(IKsign(sj1), -1);
21287  if (!x997.valid)
21288  {
21289  continue;
21290  }
21291  j2array[0] = ((-1.5707963267949) + (x996.value) + (((1.5707963267949) * (x997.value))));
21292  sj2array[0] = IKsin(j2array[0]);
21293  cj2array[0] = IKcos(j2array[0]);
21294  if (j2array[0] > IKPI)
21295  {
21296  j2array[0] -= IK2PI;
21297  }
21298  else if (j2array[0] < -IKPI)
21299  {
21300  j2array[0] += IK2PI;
21301  }
21302  j2valid[0] = true;
21303  for (int ij2 = 0; ij2 < 1; ++ij2)
21304  {
21305  if (!j2valid[ij2])
21306  {
21307  continue;
21308  }
21309  _ij2[0] = ij2;
21310  _ij2[1] = -1;
21311  for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21312  {
21313  if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21314  IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21315  {
21316  j2valid[iij2] = false;
21317  _ij2[1] = iij2;
21318  break;
21319  }
21320  }
21321  j2 = j2array[ij2];
21322  cj2 = cj2array[ij2];
21323  sj2 = sj2array[ij2];
21324  {
21325  IkReal evalcond[12];
21326  IkReal x998 = IKsin(j2);
21327  IkReal x999 = IKcos(j2);
21328  IkReal x1000 = ((1.0) * sj0);
21329  IkReal x1001 = ((1.0) * sj1);
21330  IkReal x1002 = (cj0 * new_r00);
21331  IkReal x1003 = (cj1 * sj0);
21332  IkReal x1004 = (cj0 * new_r01);
21333  IkReal x1005 = (cj1 * x999);
21334  IkReal x1006 = (cj1 * x998);
21335  evalcond[0] = (new_r21 + ((sj1 * x998)));
21336  evalcond[1] = ((((-1.0) * x1001 * x999)) + new_r20);
21337  evalcond[2] = ((((-1.0) * new_r00 * x1000)) + ((cj0 * new_r10)) + x998);
21338  evalcond[3] = ((((-1.0) * new_r01 * x1000)) + ((cj0 * new_r11)) + x999);
21339  evalcond[4] = (x1005 + x1002 + ((new_r10 * sj0)));
21340  evalcond[5] = (((x1003 * x999)) + ((cj0 * x998)) + new_r10);
21341  evalcond[6] = ((((-1.0) * x1006)) + x1004 + ((new_r11 * sj0)));
21342  evalcond[7] = (((cj0 * x1005)) + (((-1.0) * x1000 * x998)) + new_r00);
21343  evalcond[8] = (((cj0 * x999)) + new_r11 + (((-1.0) * x1000 * x1006)));
21344  evalcond[9] = ((((-1.0) * cj0 * x1006)) + (((-1.0) * x1000 * x999)) + new_r01);
21345  evalcond[10] =
21346  (((new_r10 * x1003)) + (((-1.0) * new_r20 * x1001)) + ((cj1 * x1002)) + x999);
21347  evalcond[11] = ((((-1.0) * x998)) + ((new_r11 * x1003)) + (((-1.0) * new_r21 * x1001)) +
21348  ((cj1 * x1004)));
21349  if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21350  IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21351  IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21352  IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21353  IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21354  IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21355  IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21356  IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21357  IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21358  IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21359  IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21360  IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21361  {
21362  continue;
21363  }
21364  }
21365 
21366  {
21367  std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21368  vinfos[0].jointtype = 1;
21369  vinfos[0].foffset = j0;
21370  vinfos[0].indices[0] = _ij0[0];
21371  vinfos[0].indices[1] = _ij0[1];
21372  vinfos[0].maxsolutions = _nj0;
21373  vinfos[1].jointtype = 1;
21374  vinfos[1].foffset = j1;
21375  vinfos[1].indices[0] = _ij1[0];
21376  vinfos[1].indices[1] = _ij1[1];
21377  vinfos[1].maxsolutions = _nj1;
21378  vinfos[2].jointtype = 1;
21379  vinfos[2].foffset = j2;
21380  vinfos[2].indices[0] = _ij2[0];
21381  vinfos[2].indices[1] = _ij2[1];
21382  vinfos[2].maxsolutions = _nj2;
21383  vinfos[3].jointtype = 1;
21384  vinfos[3].foffset = j3;
21385  vinfos[3].indices[0] = _ij3[0];
21386  vinfos[3].indices[1] = _ij3[1];
21387  vinfos[3].maxsolutions = _nj3;
21388  vinfos[4].jointtype = 1;
21389  vinfos[4].foffset = j4;
21390  vinfos[4].indices[0] = _ij4[0];
21391  vinfos[4].indices[1] = _ij4[1];
21392  vinfos[4].maxsolutions = _nj4;
21393  vinfos[5].jointtype = 1;
21394  vinfos[5].foffset = j5;
21395  vinfos[5].indices[0] = _ij5[0];
21396  vinfos[5].indices[1] = _ij5[1];
21397  vinfos[5].maxsolutions = _nj5;
21398  vinfos[6].jointtype = 1;
21399  vinfos[6].foffset = j6;
21400  vinfos[6].indices[0] = _ij6[0];
21401  vinfos[6].indices[1] = _ij6[1];
21402  vinfos[6].maxsolutions = _nj6;
21403  std::vector<int> vfree(0);
21404  solutions.AddSolution(vinfos, vfree);
21405  }
21406  }
21407  }
21408  }
21409  }
21410  }
21411  }
21412  }
21413  }
21414  }
21415  }
21416  }
21417  }
21418  static inline void polyroots3(IkReal rawcoeffs[3 + 1], IkReal rawroots[3], int& numroots)
21419  {
21420  using std::complex;
21421  if (rawcoeffs[0] == 0)
21422  {
21423  // solve with one reduced degree
21424  polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
21425  return;
21426  }
21427  IKFAST_ASSERT(rawcoeffs[0] != 0);
21428  const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21429  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21430  complex<IkReal> coeffs[3];
21431  const int maxsteps = 110;
21432  for (int i = 0; i < 3; ++i)
21433  {
21434  coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21435  }
21436  complex<IkReal> roots[3];
21437  IkReal err[3];
21438  roots[0] = complex<IkReal>(1, 0);
21439  roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21440  err[0] = 1.0;
21441  err[1] = 1.0;
21442  for (int i = 2; i < 3; ++i)
21443  {
21444  roots[i] = roots[i - 1] * roots[1];
21445  err[i] = 1.0;
21446  }
21447  for (int step = 0; step < maxsteps; ++step)
21448  {
21449  bool changed = false;
21450  for (int i = 0; i < 3; ++i)
21451  {
21452  if (err[i] >= tol)
21453  {
21454  changed = true;
21455  // evaluate
21456  complex<IkReal> x = roots[i] + coeffs[0];
21457  for (int j = 1; j < 3; ++j)
21458  {
21459  x = roots[i] * x + coeffs[j];
21460  }
21461  for (int j = 0; j < 3; ++j)
21462  {
21463  if (i != j)
21464  {
21465  if (roots[i] != roots[j])
21466  {
21467  x /= (roots[i] - roots[j]);
21468  }
21469  }
21470  }
21471  roots[i] -= x;
21472  err[i] = abs(x);
21473  }
21474  }
21475  if (!changed)
21476  {
21477  break;
21478  }
21479  }
21480 
21481  numroots = 0;
21482  bool visited[3] = { false };
21483  for (int i = 0; i < 3; ++i)
21484  {
21485  if (!visited[i])
21486  {
21487  // might be a multiple root, in which case it will have more error than the other roots
21488  // find any neighboring roots, and take the average
21489  complex<IkReal> newroot = roots[i];
21490  int n = 1;
21491  for (int j = i + 1; j < 3; ++j)
21492  {
21493  // care about error in real much more than imaginary
21494  if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21495  {
21496  newroot += roots[j];
21497  n += 1;
21498  visited[j] = true;
21499  }
21500  }
21501  if (n > 1)
21502  {
21503  newroot /= n;
21504  }
21505  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21506  // used, need to use the sqrt
21507  if (IKabs(imag(newroot)) < tolsqrt)
21508  {
21509  rawroots[numroots++] = real(newroot);
21510  }
21511  }
21512  }
21513  }
21514  static inline void polyroots2(IkReal rawcoeffs[2 + 1], IkReal rawroots[2], int& numroots)
21515  {
21516  IkReal det = rawcoeffs[1] * rawcoeffs[1] - 4 * rawcoeffs[0] * rawcoeffs[2];
21517  if (det < 0)
21518  {
21519  numroots = 0;
21520  }
21521  else if (det == 0)
21522  {
21523  rawroots[0] = -0.5 * rawcoeffs[1] / rawcoeffs[0];
21524  numroots = 1;
21525  }
21526  else
21527  {
21528  det = IKsqrt(det);
21529  rawroots[0] = (-rawcoeffs[1] + det) / (2 * rawcoeffs[0]);
21530  rawroots[1] = (-rawcoeffs[1] - det) / (2 * rawcoeffs[0]); // rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
21531  numroots = 2;
21532  }
21533  }
21534  static inline void polyroots4(IkReal rawcoeffs[4 + 1], IkReal rawroots[4], int& numroots)
21535  {
21536  using std::complex;
21537  if (rawcoeffs[0] == 0)
21538  {
21539  // solve with one reduced degree
21540  polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
21541  return;
21542  }
21543  IKFAST_ASSERT(rawcoeffs[0] != 0);
21544  const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21545  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21546  complex<IkReal> coeffs[4];
21547  const int maxsteps = 110;
21548  for (int i = 0; i < 4; ++i)
21549  {
21550  coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21551  }
21552  complex<IkReal> roots[4];
21553  IkReal err[4];
21554  roots[0] = complex<IkReal>(1, 0);
21555  roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21556  err[0] = 1.0;
21557  err[1] = 1.0;
21558  for (int i = 2; i < 4; ++i)
21559  {
21560  roots[i] = roots[i - 1] * roots[1];
21561  err[i] = 1.0;
21562  }
21563  for (int step = 0; step < maxsteps; ++step)
21564  {
21565  bool changed = false;
21566  for (int i = 0; i < 4; ++i)
21567  {
21568  if (err[i] >= tol)
21569  {
21570  changed = true;
21571  // evaluate
21572  complex<IkReal> x = roots[i] + coeffs[0];
21573  for (int j = 1; j < 4; ++j)
21574  {
21575  x = roots[i] * x + coeffs[j];
21576  }
21577  for (int j = 0; j < 4; ++j)
21578  {
21579  if (i != j)
21580  {
21581  if (roots[i] != roots[j])
21582  {
21583  x /= (roots[i] - roots[j]);
21584  }
21585  }
21586  }
21587  roots[i] -= x;
21588  err[i] = abs(x);
21589  }
21590  }
21591  if (!changed)
21592  {
21593  break;
21594  }
21595  }
21596 
21597  numroots = 0;
21598  bool visited[4] = { false };
21599  for (int i = 0; i < 4; ++i)
21600  {
21601  if (!visited[i])
21602  {
21603  // might be a multiple root, in which case it will have more error than the other roots
21604  // find any neighboring roots, and take the average
21605  complex<IkReal> newroot = roots[i];
21606  int n = 1;
21607  for (int j = i + 1; j < 4; ++j)
21608  {
21609  // care about error in real much more than imaginary
21610  if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21611  {
21612  newroot += roots[j];
21613  n += 1;
21614  visited[j] = true;
21615  }
21616  }
21617  if (n > 1)
21618  {
21619  newroot /= n;
21620  }
21621  // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21622  // used, need to use the sqrt
21623  if (IKabs(imag(newroot)) < tolsqrt)
21624  {
21625  rawroots[numroots++] = real(newroot);
21626  }
21627  }
21628  }
21629  }
21630 };
21631 
21634 IKFAST_API bool
21635 ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
21636 {
21637  IKSolver solver;
21638  return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21639 }
21640 
21641 IKFAST_API bool ComputeIk2(const IkReal* eetrans,
21642  const IkReal* eerot,
21643  const IkReal* pfree,
21644  IkSolutionListBase<IkReal>& solutions,
21645  void* pOpenRAVEManip)
21646 {
21647  IKSolver solver;
21648  return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21649 }
21650 
21651 IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - iiwa7 (ad1a4bdb54d4d6b54bc346ee8087a8e6)>"; }
21652 
21653 IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
21654 
21655 #ifdef IKFAST_NAMESPACE
21656 } // end namespace
21657 #endif
21658 
21659 #ifndef IKFAST_NO_MAIN
21660 #include <stdio.h>
21661 #include <stdlib.h>
21662 #ifdef IKFAST_NAMESPACE
21663 using namespace IKFAST_NAMESPACE;
21664 #endif
21665 int main(int argc, char** argv)
21666 {
21667  if (argc != 12 + GetNumFreeParameters() + 1)
21668  {
21669  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
21670  "Returns the ik solutions given the transformation of the end effector specified by\n"
21671  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
21672  "There are %d free parameters that have to be specified.\n\n",
21673  GetNumFreeParameters());
21674  return 1;
21675  }
21676 
21677  IkSolutionList<IkReal> solutions;
21678  std::vector<IkReal> vfree(GetNumFreeParameters());
21679  IkReal eerot[9], eetrans[3];
21680  eerot[0] = atof(argv[1]);
21681  eerot[1] = atof(argv[2]);
21682  eerot[2] = atof(argv[3]);
21683  eetrans[0] = atof(argv[4]);
21684  eerot[3] = atof(argv[5]);
21685  eerot[4] = atof(argv[6]);
21686  eerot[5] = atof(argv[7]);
21687  eetrans[1] = atof(argv[8]);
21688  eerot[6] = atof(argv[9]);
21689  eerot[7] = atof(argv[10]);
21690  eerot[8] = atof(argv[11]);
21691  eetrans[2] = atof(argv[12]);
21692  for (std::size_t i = 0; i < vfree.size(); ++i)
21693  vfree[i] = atof(argv[13 + i]);
21694  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
21695 
21696  if (!bSuccess)
21697  {
21698  fprintf(stderr, "Failed to get ik solution\n");
21699  return -1;
21700  }
21701 
21702  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
21703  std::vector<IkReal> solvalues(GetNumJoints());
21704  for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
21705  {
21706  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
21707  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
21708  std::vector<IkReal> vsolfree(sol.GetFree().size());
21709  sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
21710  for (std::size_t j = 0; j < solvalues.size(); ++j)
21711  printf("%.15f, ", solvalues[j]);
21712  printf("\n");
21713  }
21714  return 0;
21715 }
21716 
21717 #endif
21718 TESSERACT_COMMON_IGNORE_WARNINGS_POP
21719 // clang-format on
vector
#define vector(a, b, c)
Definition: FloatMath.inl:3227
IKSolver
Definition: abb_irb2400_ikfast_solver.hpp:388
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:3219
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21514
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21418
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21534
IKSolver::j0mul
IkReal j0mul
Definition: iiwa7_ikfast_solver.hpp:490
IKSolver::cj6
IkReal cj6
Definition: iiwa7_ikfast_solver.hpp:491
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:498
IKSolver::cj100
IkReal cj100
Definition: iiwa7_ikfast_solver.hpp:496
ikfast::IkSingleDOFSolutionBase
holds the solution for a single dof
Definition: ikfast.h:52
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:72
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:103
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:259
ikfast::IkSolutionList::GetSolution
const IkSolutionBase< T > & GetSolution(size_t index) const override
returns the solution pointer
Definition: ikfast.h:268
ikfast::IkSolutionList::GetNumSolutions
size_t GetNumSolutions() const override
returns the number of solutions stored
Definition: ikfast.h:279
main
int main(int argc, char **argv)
Definition: create_convex_hull.cpp:43
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: iiwa7_ikfast_solver.hpp:21641
IKPI
#define IKPI
Definition: iiwa7_ikfast_solver.hpp:77
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: iiwa7_ikfast_solver.hpp:147
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: iiwa7_ikfast_solver.hpp:33
IKasin
float IKasin(float f)
Definition: iiwa7_ikfast_solver.hpp:161
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: iiwa7_ikfast_solver.hpp:476
dgetrs_
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: iiwa7_ikfast_solver.hpp:240
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: iiwa7_ikfast_solver.hpp:277
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: iiwa7_ikfast_solver.hpp:481
IKPI_2
#define IKPI_2
Definition: iiwa7_ikfast_solver.hpp:78
IKatan2
float IKatan2(float fy, float fx)
Definition: iiwa7_ikfast_solver.hpp:241
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: iiwa7_ikfast_solver.hpp:141
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: iiwa7_ikfast_solver.hpp:483
IKacos
float IKacos(float f)
Definition: iiwa7_ikfast_solver.hpp:202
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: iiwa7_ikfast_solver.hpp:158
IKsqrt
float IKsqrt(float f)
Definition: iiwa7_ikfast_solver.hpp:228
IKsin
float IKsin(float f)
Definition: iiwa7_ikfast_solver.hpp:222
IKsign
float IKsign(float f)
Definition: iiwa7_ikfast_solver.hpp:293
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKfmod
float IKfmod(float x, float y)
Definition: iiwa7_ikfast_solver.hpp:183
IKcos
float IKcos(float f)
Definition: iiwa7_ikfast_solver.hpp:224
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: iiwa7_ikfast_solver.hpp:320
IKsqr
float IKsqr(float f)
Definition: iiwa7_ikfast_solver.hpp:133
GetIkType
IKFAST_API int GetIkType()
Definition: iiwa7_ikfast_solver.hpp:485
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: iiwa7_ikfast_solver.hpp:381
IKtan
float IKtan(float f)
Definition: iiwa7_ikfast_solver.hpp:226
IK2PI
#define IK2PI
Definition: iiwa7_ikfast_solver.hpp:76
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:21635
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: iiwa7_ikfast_solver.hpp:152
IKlog
float IKlog(float f)
Definition: iiwa7_ikfast_solver.hpp:136
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: iiwa7_ikfast_solver.hpp:21653
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: iiwa7_ikfast_solver.hpp:475
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: iiwa7_ikfast_solver.hpp:57
IKabs
float IKabs(float f)
Definition: iiwa7_ikfast_solver.hpp:130
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: iiwa7_ikfast_solver.hpp:21651
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
macros.h
Common Tesseract Macros.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: create_convex_hull.cpp:36
ikfast
Definition: ikfast.h:48
tesseract_collision::FLOAT_MATH::det
REAL det(const REAL *p1, const REAL *p2, const REAL *p3)
Definition: FloatMath.inl:3319
CheckValue
Definition: iiwa7_ikfast_solver.hpp:271
CheckValue::valid
bool valid
Definition: iiwa7_ikfast_solver.hpp:273
CheckValue::value
T value
Definition: iiwa7_ikfast_solver.hpp:272
Generated on Mon Jan 10 2022 22:20:37 for Tesseract by doxygen 1.9.1